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

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

Simulator::~Simulator()
{
pitiot's avatar
maj  
pitiot 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
}

pitiot's avatar
pitiot committed
31
void Simulator::init( float dimension, bool enablePathFinding)
Thomas's avatar
Thomas committed
32
{
33
	std::cout << "Setup scenario" << std::endl ;
pitiot's avatar
pitiot committed
34

pitiot's avatar
pitiot committed
35 36 37 38 39
#ifndef SPATIAL_HASHING
//	envMap_.subdivideToProperLevel() ;
//	envMap_.subdivideAllToMaxLevel();
#endif

David Cazier's avatar
David Cazier committed
40 41
	switch (config)
	{
pitiot's avatar
maj  
pitiot committed
42
		case 0 :
pitiot's avatar
pitiot committed
43
			setupCircleScenario(1000,40) ;
pitiot's avatar
maj  
pitiot committed
44 45
			break ;
		case 1 :
pitiot's avatar
pitiot committed
46
			setupCorridorScenario(1000,40) ;
pitiot's avatar
maj  
pitiot committed
47
			break ;
48 49 50
		case 2 :
			setupSnakeCorridorScenario(1000,5,10) ;
			break ;
pitiot's avatar
pitiot committed
51 52 53 54 55 56 57 58
//		case 3 :
//			setupCityScenario(20, 20) ;
////			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
59 60
	}

pitiot's avatar
maj  
pitiot committed
61 62 63 64 65
#ifndef SPATIAL_HASHING
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
		envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d) ;
#endif

Pierre Kraemer's avatar
Pierre Kraemer committed
66
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
67 68 69 70
	if (enablePathFinding)
	{
		if (dimension == 2.0f)
			addPathsToAgents() ;
71
		else if (dimension == 2.5f) addPathsToAgents_height() ;
Thomas's avatar
Thomas committed
72
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
73
#endif
74

pitiot's avatar
maj  
pitiot committed
75
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
76
	{
David Cazier's avatar
David Cazier committed
77 78
		agents_[i]->updateObstacleNeighbors() ;
		agents_[i]->updateAgentNeighbors() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
79
	}
80

pitiot's avatar
pitiot committed
81

Pierre Kraemer's avatar
Pierre Kraemer committed
82
#ifndef SPATIAL_HASHING
pitiot's avatar
pitiot committed
83 84
	if (multires)
		envMap_.subdivideToProperLevel() ;
pitiot's avatar
maj  
pitiot committed
85
//	envMap_.subdivideAllToMaxLevel();
Pierre Kraemer's avatar
Pierre Kraemer committed
86
#endif
pitiot's avatar
maj  
pitiot committed
87 88

//	setupMovingObstacle(40);
pitiot's avatar
pitiot committed
89

Pierre Kraemer's avatar
Pierre Kraemer committed
90 91
}

pitiot's avatar
maj  
pitiot committed
92
void Simulator::doStep()
Pierre Kraemer's avatar
Pierre Kraemer committed
93
{
pitiot's avatar
maj  
pitiot committed
94 95
#ifndef SPATIAL_HASHING
	envMap_.clearUpdateCandidates() ;
pitiot's avatar
pitiot committed
96
	envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
pitiot's avatar
maj  
pitiot committed
97 98 99 100 101
#endif

	for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i)
	{
		movingObstacles_[i]->computePrefVelocity() ;
pitiot's avatar
pitiot committed
102
		movingObstacles_[i]->computeNewVelocity() ;
pitiot's avatar
pitiot committed
103 104 105
#ifndef SPATIAL_HASHING
		envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
#endif
pitiot's avatar
maj  
pitiot committed
106 107
		movingObstacles_[i]->update() ;
	}
pitiot's avatar
pitiot committed
108

pitiot's avatar
maj  
pitiot committed
109
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
110
	{
pitiot's avatar
pitiot committed
111 112 113
		if (agents_[i]->alive)
		{
			agents_[i]->updateObstacleNeighbors() ;
pitiot's avatar
maj  
pitiot committed
114 115

#ifndef SPATIAL_HASHING
pitiot's avatar
pitiot committed
116
			envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
pitiot's avatar
maj  
pitiot committed
117
#endif
pitiot's avatar
pitiot committed
118
			agents_[i]->updateAgentNeighbors() ;
pitiot's avatar
maj  
pitiot committed
119
#ifndef SPATIAL_HASHING
pitiot's avatar
pitiot committed
120
			envMap_.map.setCurrentLevel(0) ;
pitiot's avatar
maj  
pitiot committed
121 122
#endif

pitiot's avatar
pitiot committed
123 124 125
			agents_[i]->computePrefVelocity() ;
			agents_[i]->computeNewVelocity() ;
		}
pitiot's avatar
maj  
pitiot committed
126
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
127

Pierre Kraemer's avatar
Pierre Kraemer committed
128
#ifdef SPATIAL_HASHING
pitiot's avatar
maj  
pitiot committed
129 130 131 132 133 134 135 136 137 138 139 140
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	{
		Geom::Vec2ui cold = envMap_.agentPositionCell(agents_[i]) ;
		agents_[i]->update() ;
		Geom::Vec2ui cnew = envMap_.agentPositionCell(agents_[i]) ;
		if (cnew != cold)
		{
			envMap_.removeAgentFromGrid(agents_[i], cold) ;
			envMap_.addAgentInGrid(agents_[i], cnew) ;
		}
	}
#else
141
	if (multires) envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
pitiot's avatar
maj  
pitiot committed
142 143 144 145
	nb_dead = 0 ;
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	{
		if (agents_[i]->alive)
Pierre Kraemer's avatar
Pierre Kraemer committed
146
		{
pitiot's avatar
maj  
pitiot committed
147 148 149 150 151 152
			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)
Pierre Kraemer's avatar
Pierre Kraemer committed
153
			{
pitiot's avatar
maj  
pitiot committed
154 155 156 157 158 159
//			case CGoGN::Algo::MovingObjects::CROSS_EDGE :
//				envMap_.agentChangeFaceThroughEdge(agents_[i]);
//				break;
//			case CGoGN::Algo::MovingObjects::CROSS_OTHER :
				envMap_.agentChangeFace(agents_[i], oldFace) ;
//				break;
Pierre Kraemer's avatar
Pierre Kraemer committed
160 161
			}
		}
pitiot's avatar
maj  
pitiot committed
162 163 164 165 166 167 168
		else
			nb_dead++ ;

	}

	nbRefineCandidate += envMap_.refineCandidate.size() ;
	nbCoarsenCandidate += envMap_.coarsenCandidate.size() ;
pitiot's avatar
pitiot committed
169 170 171

	if (multires)
		envMap_.updateMap() ;
pitiot's avatar
maj  
pitiot committed
172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187
#endif

	globalTime_ += timeStep_ ;
	++nbSteps_ ;
}

bool Simulator::reachedGoal()
{
	/* Check if all agents have reached their goals. */
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
		if ((agents_[i]->getPosition() - agents_[i]->goals_[agents_[i]->curGoal_]).norm2()
		    > agents_[i]->radius_ * agents_[i]->radius_) return false ;

	return true ;
}

188
void Simulator::addAgent(const VEC3& start, const std::vector<VEC3>& goal)
pitiot's avatar
maj  
pitiot committed
189 190 191 192 193
{
#ifdef SPATIAL_HASHING
	Agent* a = new Agent(this, start) ;
	agents_.push_back(a) ;
	envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
194
#else
pitiot's avatar
pitiot committed
195

pitiot's avatar
maj  
pitiot committed
196
	Dart dCell = envMap_.getBelongingCell(start) ;
pitiot's avatar
pitiot committed
197
//	if (dCell==envMap_.map.begin()) CGoGNout<<"pbagents"<<CGoGNendl;
pitiot's avatar
maj  
pitiot committed
198
	agents_.push_back(new Agent(this, start, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
199
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
200

201
	agents_.back()->goals_=goal ;
pitiot's avatar
maj  
pitiot committed
202 203 204
	agents_.back()->curGoal_ = 0 ;
}

pitiot's avatar
pitiot committed
205
void Simulator::setupCircleScenario(unsigned int nbAgents , unsigned int nbObstacles)
pitiot's avatar
maj  
pitiot committed
206
{
pitiot's avatar
pitiot committed
207 208 209 210 211 212 213 214 215 216
	if (multires)
	{
		envMap_.init(config, 2000.0f, 2000.0f, minSize, 400.0f) ; //grosses cases
	}
	else
	{

		envMap_.init(config, 1600.0f, 960.0f, minSize, 20.0f) ; //cases fines
	}

pitiot's avatar
maj  
pitiot committed
217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238
	std::cout << " - Setup Circle Scenario : " << nbAgents << " agents" << std::endl ;

	// Bordure à éviter autour de la scène (10% de sa taille)
	int xBorder = envMap_.geometry.size(0) / 10 ;
	int yBorder = envMap_.geometry.size(1) / 10 ;

	// Les coordonnées sont comprises entre xMin et xMin+xDelta

	// Départ des agents sur un cercle centré
	int xSize = envMap_.geometry.size(0) - 2 * xBorder ;
	int ySize = envMap_.geometry.size(1) - 2 * yBorder ;
	int radius = min(xSize, ySize) / 2 ;
	VEC3 center = (envMap_.geometry.min() + envMap_.geometry.max()) / 2 ;

	double pi = 3.14159265358979323846f ;

	for (unsigned int i = 0 ; i < nbAgents ; ++i)
	{
		double angle = i * 2.0f * pi / float(nbAgents) ;
		VEC3 v(std::cos(angle) * radius, std::sin(angle) * radius, 0) ;
		VEC3 start = center + v ;
		VEC3 goal = center - v ;
239 240 241 242
		std::vector<VEC3> goals;
		goals.push_back(start);
		goals.push_back(goal);
		addAgent(start, goals) ;
243
	}
pitiot's avatar
pitiot committed
244 245 246 247 248
	VEC3 xSide (5.0f,0.0f,0.0f);
	VEC3 ySide (0.0f,10.0f,0.0f);
	std::vector<VEC3> vPos;
	MovingObstacle* mo4;
	std::vector<VEC3> positions;
pitiot's avatar
pitiot committed
249
	for (unsigned int i = 0 ; i < nbObstacles ; i++)
pitiot's avatar
maj  
pitiot committed
250
	{
pitiot's avatar
pitiot committed
251 252 253 254 255 256 257 258 259 260 261 262
		double angle = i * 2.0f * pi / float(nbObstacles/2) ;
		VEC3 v,start;
		if(i<nbObstacles/2)
		{
			v=VEC3 (std::cos(angle) * (2*radius/3), std::sin(angle) * (2*radius/3), 0) ;
		}
		else
		{
			v=VEC3 (std::cos(angle) * (radius/3), std::sin(angle) * (radius/3), 0) ;
		}
		start = center + v ;
		positions.push_back(start);
pitiot's avatar
pitiot committed
263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279
	}
	for (unsigned int i = 0 ; i < nbObstacles/2 ; i++)
	{
		VEC3 start = positions[i];
		std::vector<VEC3> goals;
		for(unsigned int k = 0 ; k < nbObstacles/2 ; k++)
		{
			goals.push_back(positions[(i+k)%(nbObstacles/2)]);
		}

		vPos.clear();
		// Un obstacle sur deux va vers le haut
		vPos.push_back(start+xSide-ySide);
		vPos.push_back(start+xSide+ySide);
		vPos.push_back(start-xSide+ySide);
		vPos.push_back(start-xSide-ySide);

pitiot's avatar
pitiot committed
280
		mo4= new MovingObstacle(this, i,vPos,goals,true);
pitiot's avatar
pitiot committed
281

pitiot's avatar
pitiot committed
282 283
		movingObstacles_.push_back(mo4);
	}
pitiot's avatar
pitiot committed
284

pitiot's avatar
pitiot committed
285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301
	for (unsigned int i = nbObstacles/2 ; i < nbObstacles ; i++)
	{
		VEC3 start = positions[i];
		std::vector<VEC3> goals;
		for(unsigned int k = (nbObstacles/2) ; k > 0 ; k--)
		{
			goals.push_back(positions[((i-(nbObstacles/2)+k)%(nbObstacles/2))+nbObstacles/2]);
		}

		vPos.clear();
		// Un obstacle sur deux va vers le haut
		vPos.push_back(start+xSide-ySide);
		vPos.push_back(start+xSide+ySide);
		vPos.push_back(start-xSide+ySide);
		vPos.push_back(start-xSide-ySide);

		mo4= new MovingObstacle(this, i,vPos,goals,true);
pitiot's avatar
pitiot committed
302 303

		movingObstacles_.push_back(mo4);
pitiot's avatar
maj  
pitiot committed
304
	}
pitiot's avatar
pitiot committed
305

306
#ifndef SPATIAL_HASHING
pitiot's avatar
maj  
pitiot committed
307 308
	envMap_.clearUpdateCandidates() ;
	envMap_.map.setCurrentLevel(0) ;
309
#endif
pitiot's avatar
maj  
pitiot committed
310 311

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

pitiot's avatar
maj  
pitiot committed
314
void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles)
Thomas's avatar
Thomas committed
315
{
pitiot's avatar
pitiot committed
316 317 318 319 320 321 322 323 324 325 326
	if (multires)
	{
		envMap_.init(config, 1600.0f, 960.0f, minSize, 320.0f) ; //grosses cases
	}
	else
	{

		envMap_.init(config, 1600.0f, 960.0f, minSize, 20.0f) ; //cases fines
	}


pitiot's avatar
maj  
pitiot committed
327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347
	std::cout << " - Setup Corridor Scenario : " << nbAgents << " agents et " << nbObstacles << " obstacles" << std::endl ;

	// Bordure à éviter autour de la scène (10% de sa taille)
	int xBorder = envMap_.geometry.size(0) / 10 ;
	int yBorder = envMap_.geometry.size(1) / 10 ;

	// Les coordonnées sont comprises entre xMin et xMin+xDelta

	// Départ des agents du quart gauche sur toute la hauteur
	int xStartMin = envMap_.geometry.min()[0] + xBorder ;
	int xStartDelta = envMap_.geometry.size(0) / 5 ;
	int yStartMin = envMap_.geometry.min()[1] + yBorder ;
	int yStartDelta = envMap_.geometry.size(1) - 2 * yBorder ;

	// Arrivée des agents à l'opposée
	int xGoalDelta = envMap_.geometry.size(0) / 5 ;
	int xGoalMin = envMap_.geometry.max()[0] - xBorder - xGoalDelta ;
	int yGoalMin = yStartMin ;
	int yGoalDelta = yStartDelta ;

	for (unsigned int i = 0 ; i < nbAgents ; ++i)
David Cazier's avatar
David Cazier committed
348
	{
pitiot's avatar
maj  
pitiot committed
349 350
		VEC3 start(xStartMin + rand() % xStartDelta, yStartMin + rand() % yStartDelta, 0) ;
		VEC3  goal(xGoalMin  + rand() % xGoalDelta,  yGoalMin  + rand() % yGoalDelta,  0) ;
351

pitiot's avatar
maj  
pitiot committed
352 353
		// Un agent sur 2 va de droite à gauche
		VEC3 tmp ;
David Cazier's avatar
David Cazier committed
354 355
		if (i % 2 == 1)
		{
pitiot's avatar
maj  
pitiot committed
356 357 358
			tmp = goal ;
			goal = start ;
			start = tmp ;
Pierre Kraemer's avatar
Pierre Kraemer committed
359
		}
360 361 362 363
		std::vector<VEC3> goals;
		goals.push_back(start);
		goals.push_back(goal);
		addAgent(start, goals) ;
Thomas's avatar
Thomas committed
364 365
	}

pitiot's avatar
maj  
pitiot committed
366 367 368 369 370
	// Départ des obstacles du quart haut sur toute une demi-largeur
	xStartMin = envMap_.geometry.min()[0] + envMap_.geometry.size(0) / 4 ;
	xStartDelta = envMap_.geometry.size(0) / 2 ;
	yStartMin = envMap_.geometry.min()[1] + yBorder ;
	yStartDelta = envMap_.geometry.size(1) / 5 ;
Thomas's avatar
Thomas committed
371

pitiot's avatar
maj  
pitiot committed
372 373 374 375 376 377 378 379 380 381 382 383
	// Arrivée des obstacles à l'opposée
	yGoalDelta = envMap_.geometry.size(1) / 5 ;
	yGoalMin = envMap_.geometry.max()[1] - yBorder - yGoalDelta ;

	VEC3 xSide (5.0f,0.0f,0.0f);
	VEC3 ySide (0.0f,10.0f,0.0f);
	std::vector<VEC3> vPos;
	MovingObstacle* mo4;

	for (unsigned int i = 0 ; i < nbObstacles ; i++)
	{
		float x = xStartMin + ((int)i*30) % xStartDelta;
384
//		std::cout << "x ? " << x << " xStartDelta " << xStartDelta << std::endl;
pitiot's avatar
maj  
pitiot committed
385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406
		VEC3 start(x, yStartMin + rand() % yStartDelta, 0) ;
		VEC3  goal(x, yGoalMin  + rand() % yGoalDelta,  0) ;

		vPos.clear();
		// Un obstacle sur deux va vers le haut
		VEC3 tmp ;
		if (i % 2 == 1)
		{
			tmp = goal ;
			goal = start ;
			start = tmp ;
			vPos.push_back(start-xSide+ySide);
			vPos.push_back(start-xSide-ySide);
			vPos.push_back(start+xSide-ySide);
			vPos.push_back(start+xSide+ySide);
		}
		else {
			vPos.push_back(start+xSide-ySide);
			vPos.push_back(start+xSide+ySide);
			vPos.push_back(start-xSide+ySide);
			vPos.push_back(start-xSide-ySide);
		}
pitiot's avatar
pitiot committed
407 408 409
		std::vector<VEC3> goals;
		goals.push_back(start);
		goals.push_back(goal);
pitiot's avatar
pitiot committed
410
		mo4= new MovingObstacle(this, i,vPos,goals,false);
pitiot's avatar
pitiot committed
411 412 413 414 415


		//for generating a random path
//		unsigned int dartDistForPath = 50 ;
//		mo4->goals_.clear() ;
pitiot's avatar
pitiot committed
416
//		Dart dStart = mo4->registering_part->d;
pitiot's avatar
pitiot committed
417 418 419 420 421 422 423 424 425 426 427
//		Dart dStop = dStart ;
//		for (unsigned int j = 0 ; envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath + rand() * 20 || envMap_.map.sameFace(dStop, dStart) ; ++j)
//		{
//			envMap_.map.next(dStop) ;
//			if (dStop == envMap_.map.end())
//				dStop = envMap_.map.begin() ;
//		}
//
//		addPathToObstacle(mo4, dStart, dStop);
//		addPathToObstacle(mo4, dStop, dStart);

pitiot's avatar
maj  
pitiot committed
428 429
		movingObstacles_.push_back(mo4);
	}
Thomas's avatar
Thomas committed
430 431
}

432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498 499 500 501 502 503 504 505 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 556
void Simulator::setupSnakeCorridorScenario(unsigned int nbAgents, unsigned int nbSnakes, int snakeSize)
{
	if (multires)
	{
		envMap_.init(config, 1600.0f, 960.0f, minSize, 320.0f) ; //grosses cases
	}
	else
	{

		envMap_.init(config, 1600.0f, 960.0f, minSize, 20.0f) ; //cases fines
	}


	std::cout << " - Setup Snake Corridor Scenario : " << nbAgents << " agents et " << nbSnakes*snakeSize << " obstacles" << std::endl ;

	// Bordure à éviter autour de la scène (10% de sa taille)
	int xBorder = envMap_.geometry.size(0) / 5 ;
	int yBorder = envMap_.geometry.size(1) / 5 ;

	// Les coordonnées sont comprises entre xMin et xMin+xDelta

	// Départ des agents du quart gauche sur toute la hauteur
	int xStartMin = envMap_.geometry.min()[0] + xBorder ;
	int xStartDelta = envMap_.geometry.size(0) / 5 ;
	int yStartMin = envMap_.geometry.min()[1] + yBorder ;
	int yStartDelta = envMap_.geometry.size(1) - 2 * yBorder ;

	// Arrivée des agents à l'opposée
	int xGoalDelta = envMap_.geometry.size(0) / 5 ;
	int xGoalMin = envMap_.geometry.max()[0] - xBorder - xGoalDelta ;
	int yGoalMin = yStartMin ;
	int yGoalDelta = yStartDelta ;

	for (unsigned int i = 0 ; i < nbAgents ; ++i)
	{
		VEC3 start(xStartMin + rand() % xStartDelta, yStartMin + rand() % yStartDelta, 0) ;
		VEC3  goal(xGoalMin  + rand() % xGoalDelta,  yGoalMin  + rand() % yGoalDelta,  0) ;

		// Un agent sur 2 va de droite à gauche
		VEC3 tmp ;
		if (i % 2 == 1)
		{
			tmp = goal ;
			goal = start ;
			start = tmp ;
		}
		std::vector<VEC3> goals;
		goals.push_back(start);
		goals.push_back(goal);
		addAgent(start, goals) ;
	}

	// Départ des obstacles du quart haut sur toute une demi-largeur
	xStartMin = envMap_.geometry.min()[0] + envMap_.geometry.size(0) / 4 ;
	xStartDelta = envMap_.geometry.size(0) / 2 ;
	yStartMin = envMap_.geometry.min()[1] + yBorder ;
	yStartDelta = envMap_.geometry.size(1) / 20 ;

	// Arrivée des obstacles à l'opposée
	yGoalDelta = envMap_.geometry.size(1) / 5 ;
	yGoalMin = envMap_.geometry.max()[1] - yBorder - yGoalDelta ;

	VEC3 xSide (5.0f,0.0f,0.0f);
	VEC3 ySide (0.0f,10.0f,0.0f);


	int sumObstacles=0;
	for(unsigned int j = 0; j<nbSnakes; j++)
	{
		std::vector<PFP::VEC3> positions [snakeSize] ;
		float x = xStartMin + rand() % xStartDelta;
		VEC3 goal;

		VEC3 start(x, yStartMin + rand() % yStartDelta, 0) ;


		std::vector<VEC3> vPos;


		vPos.push_back(start+xSide-ySide);
		vPos.push_back(start+xSide+ySide);
		vPos.push_back(start-xSide+ySide);
		vPos.push_back(start-xSide-ySide);

		std::vector<VEC3> goals;
		goals.push_back(start);
		int r=0;
		while (r<40)
		{
			x = xStartMin + rand() % xStartDelta;
			goal=VEC3 (x, yGoalMin  + rand() % yGoalDelta,  0) ;
			if ((goal-goals[r]).norm2()>1000){
				goals.push_back(goal);
							r++;
			}

		}

		positions[0]=vPos;

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

		{

			start=start-ySide-ySide;

			vPos.clear();


			vPos.push_back(start+xSide-ySide);
			vPos.push_back(start+xSide+ySide);
			vPos.push_back(start-xSide+ySide);
			vPos.push_back(start-xSide-ySide);
			positions[i]=vPos;
		}
//		CGoGNout<<"positions : "<< positions[0][0]<<CGoGNendl;
		ArticulatedObstacle * art=new ArticulatedObstacle (this,j,sumObstacles,positions,snakeSize,goals);
		sumObstacles += snakeSize;
		for (int i = 0 ; i < snakeSize ; i++)
		{
			movingObstacles_.push_back(art->members[i]);
		}
	}
}

pitiot's avatar
maj  
pitiot committed
557
void Simulator::setupCityScenario(int nbLines, int nbRank)
Thomas's avatar
Thomas committed
558
{
pitiot's avatar
maj  
pitiot committed
559 560 561 562 563
	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
564

pitiot's avatar
maj  
pitiot committed
565
	VEC3 posagent(xCornerDown - xBorder / 2, yCornerDown - yBorder / 2, 0.0f) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
566 567 568
#ifdef SPATIAL_HASHING
	Agent* a = new Agent(this, posagent) ;
	agents_.push_back(a) ;
pitiot's avatar
maj  
pitiot committed
569
	envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
570
#else
571 572
	Dart dCell = envMap_.getBelongingCell(posagent) ;
	agents_.push_back(new Agent(this, posagent, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
573
#endif
David Cazier's avatar
David Cazier committed
574
	agents_.back()->goals_.push_back(-1.0f * posagent) ;
575
	agents_.back()->curGoal_ = 0 ;
576

pitiot's avatar
maj  
pitiot committed
577
	for (int i = 0 ; i < nbLines ; ++i)
David Cazier's avatar
David Cazier committed
578
	{
pitiot's avatar
maj  
pitiot committed
579
		for (int j = 0 ; j < nbRank ; ++j)
David Cazier's avatar
David Cazier committed
580
		{
581
			VEC3 posagent(xCornerDown + i * 10.0f, yCornerDown + j * 10.0f, 0.0f) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
582 583 584
#ifdef SPATIAL_HASHING
			Agent* a = new Agent(this, posagent) ;
			agents_.push_back(a) ;
pitiot's avatar
maj  
pitiot committed
585
			envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
586
#else
587 588
			Dart dCell = envMap_.getBelongingCell(posagent) ;
			agents_.push_back(new Agent(this, posagent, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
589
#endif
David Cazier's avatar
David Cazier committed
590
			agents_.back()->goals_.push_back(-1.0f * posagent) ;
591
			agents_.back()->curGoal_ = 0 ;
Pierre Kraemer's avatar
yeah..  
Pierre Kraemer committed
592
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
593
	}
Thomas's avatar
Thomas committed
594

David Cazier's avatar
David Cazier committed
595
	std::cout << "nb agents : " << agents_.size() << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
596 597
}

598
void Simulator::setupScenario(unsigned int nbMaxAgent)
Pierre Kraemer's avatar
Pierre Kraemer committed
599
{
600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617 618 619 620
//	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
621
	/*
David Cazier's avatar
David Cazier committed
622 623 624 625
	 * 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
maj  
pitiot committed
626
	CellMarker<FACE> filled(envMap_.map) ;
David Cazier's avatar
David Cazier committed
627 628 629 630 631 632

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

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

pitiot's avatar
maj  
pitiot committed
633
	for (unsigned int i = 0 ; i < bMax && d != envMap_.map.end() ; ++i)
David Cazier's avatar
David Cazier committed
634
	{
David Cazier's avatar
David Cazier committed
635 636 637
		bool found = false ;
		VEC3 pos ;
		Dart dCell ;
David Cazier's avatar
David Cazier committed
638 639 640 641 642
		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
643
				filled.mark(d) ;
644
				pos = Algo::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
David Cazier's avatar
David Cazier committed
645 646 647
				pos[2] = 0 ;
				dCell = d ;
				found = true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
648
			}
David Cazier's avatar
David Cazier committed
649
			envMap_.map.next(d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
650 651
		}

David Cazier's avatar
David Cazier committed
652 653
		if (found)
		{
654 655
			float ecart = 3.0f ;
			VEC3 posinit = VEC3(pos[0] - (float(nbx) / 2.0f * ecart),
David Cazier's avatar
David Cazier committed
656
			                    pos[1] - (float(nby) / 2.0f * ecart), pos[2]) ;
pitiot's avatar
maj  
pitiot committed
657
			for (unsigned int curx = 0 ; curx < nbx ; ++curx)
David Cazier's avatar
David Cazier committed
658
			{
pitiot's avatar
maj  
pitiot committed
659
				for (unsigned int cury = 0 ; cury < nby ; ++cury)
David Cazier's avatar
David Cazier committed
660
				{
661
					VEC3 posagent = posinit + VEC3(ecart * curx, ecart * cury, 0.0f) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
662 663 664
#ifdef SPATIAL_HASHING
					Agent* a = new Agent(this, posagent) ;
					agents_.push_back(a) ;
pitiot's avatar
maj  
pitiot committed
665
					envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
666
#else
667
					agents_.push_back(new Agent(this, posagent, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
668
#endif
669
					agents_.back()->goals_.push_back(posagent) ;
David Cazier's avatar
David Cazier committed
670
					agents_.back()->goals_.push_back(-1.0f * posagent) ;
pitiot's avatar
maj  
pitiot committed
671

672
					agents_.back()->curGoal_ = 1 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
673 674 675 676
				}
			}
		}
	}
David Cazier's avatar
David Cazier committed
677 678
	std::cout << "nb agents : " << agents_.size() << std::endl ;
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
679
}
680

pitiot's avatar
pitiot committed
681 682 683 684 685 686 687 688 689 690 691 692 693 694 695
void Simulator::addPathToObstacle(MovingObstacle * mo, Dart dStart, Dart dGoal)
{
	std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
																   envMap_.position, dStart,
																   dGoal,
																   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) ;

		mo->goals_.push_back(dest) ;
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
696
#ifndef SPATIAL_HASHING
697 698
void Simulator::addPathToCorner()
{
pitiot's avatar
maj  
pitiot committed
699
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
700
	{
David Cazier's avatar
David Cazier committed
701 702
		agents_[i]->goals_.clear() ;
		agents_.back()->curGoal_ = 1 ;
703

David Cazier's avatar
David Cazier committed
704 705
		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = agents_[i]->finalDart ;
706

707
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
David Cazier's avatar
David Cazier committed
708 709 710
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
711

pitiot's avatar
maj  
pitiot committed
712
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
713
		{
714
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
715 716 717 718 719

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

David Cazier's avatar
David Cazier committed
720
			agents_[i]->goals_.push_back(dest) ;
721 722 723 724
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
725 726
void Simulator::addPathsToAgents()
{
Thomas's avatar
Thomas committed
727
	//city
David Cazier's avatar
David Cazier committed
728
	unsigned int dartDistForPath = 50 ;
pitiot's avatar
maj  
pitiot committed
729
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
730
	{
David Cazier's avatar
David Cazier committed
731 732 733 734
		agents_[i]->goals_.clear() ;

		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
pitiot's avatar
maj  
pitiot committed
735
		for (unsigned int j = 0 ; /*!envMap_.pedWayMark.isMarked(dStop) ||*/
David Cazier's avatar
David Cazier committed
736
		    envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath + rand() * 20
pitiot's avatar
maj  
pitiot committed
737
		        || envMap_.map.sameFace(dStop, dStart) ; ++j)
David Cazier's avatar
David Cazier committed
738
		{
David Cazier's avatar
David Cazier committed
739 740
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end()) dStop = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
741
		}
742

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

745
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
David Cazier's avatar
David Cazier committed
746 747 748
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
749

pitiot's avatar
maj  
pitiot committed
750
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
751
		{
752
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
753
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
754 755
			dest /= 2.0f ;
			dest[2] = 0 ;
756

David Cazier's avatar
David Cazier committed
757
			agents_[i]->goals_.push_back(dest) ;
758
		}
759

David Cazier's avatar
David Cazier committed
760
		Dart dStop2 = dStop ;
pitiot's avatar
maj  
pitiot committed
761
		for (unsigned int j = 0 ; /*!envMap_.pedWayMark.isMarked(dStop) ||*/
David Cazier's avatar
David Cazier committed
762
		    envMap_.buildingMark.isMarked(dStop2) || j < dartDistForPath + rand() * 20
pitiot's avatar
maj  
pitiot committed
763 764
		        || envMap_.map.sameFace(dStop, dStop2) || envMap_.map.sameFace(dStop2, dStart) ;
		    ++j)
David Cazier's avatar
David Cazier committed
765
		{
David Cazier's avatar
David Cazier committed
766 767
			envMap_.map.next(dStop2) ;
			if (dStop2 == envMap_.map.end()) dStop2 = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
768
		}
769

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

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

pitiot's avatar
maj  
pitiot committed
775
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
776
		{
777
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
778
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
779 780
			dest /= 2.0f ;
			dest[2] = 0 ;
781

David Cazier's avatar
David Cazier committed
782
			agents_[i]->goals_.push_back(dest) ;
783
		}
784

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

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

pitiot's avatar
maj  
pitiot committed
790
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
791
		{
792
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
793
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
794 795
			dest /= 2.0f ;
			dest[2] = 0 ;
796

David Cazier's avatar
David Cazier committed
797
			agents_[i]->goals_.push_back(dest) ;
798
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
799 800
	}
}
801

Thomas's avatar
Thomas committed
802 803 804
void Simulator::addPathsToAgents_height()
{
	//city
David Cazier's avatar
David Cazier committed
805
	unsigned int dartDistForPath = 50 ;
pitiot's avatar
maj  
pitiot committed
806
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
807
	{
David Cazier's avatar
David Cazier committed
808 809 810 811
		agents_[i]->goals_.clear() ;

		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
pitiot's avatar
maj  
pitiot committed
812
		for (unsigned int j = 0 ;
David Cazier's avatar
David Cazier committed
813
		    envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath + rand() * 20
pitiot's avatar
maj  
pitiot committed
814
		        || envMap_.map.sameFace(dStop, dStart) ; ++j)
David Cazier's avatar
David Cazier committed
815
		{
David Cazier's avatar
David Cazier committed
816 817
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end()) dStop = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
818 819
		}

820
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
David Cazier's avatar
David Cazier committed
821 822 823
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
Thomas's avatar
Thomas committed
824

pitiot's avatar
maj  
pitiot committed
825
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
826
		{
827
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
David Cazier's avatar
David Cazier committed
828
			agents_[i]->goals_.push_back(dest) ;
Thomas's avatar
Thomas committed
829 830
		}

David Cazier's avatar
David Cazier committed
831
		Dart dStop2 = dStop ;
pitiot's avatar
maj  
pitiot committed
832
		for (unsigned int j = 0 ;
David Cazier's avatar
David Cazier committed
833
		    envMap_.buildingMark.isMarked(dStop2) || j < dartDistForPath + rand() * 20
pitiot's avatar
maj  
pitiot committed
834 835
		        || envMap_.map.sameFace(dStop, dStop2) || envMap_.map.sameFace(dStop2, dStart) ;
		    ++j)
David Cazier's avatar
David Cazier committed
836
		{
David Cazier's avatar
David Cazier committed
837 838
			envMap_.map.next(dStop2) ;
			if (dStop2 == envMap_.map.end()) dStop2 = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
839 840
		}

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

pitiot's avatar
maj  
pitiot committed
844
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
845
		{
846
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
David Cazier's avatar
David Cazier committed
847
			agents_[i]->goals_.push_back(dest) ;
Thomas's avatar
Thomas committed
848 849
		}

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

pitiot's avatar
maj  
pitiot committed
853
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
854
		{
855
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
David Cazier's avatar
David Cazier committed
856
			agents_[i]->goals_.push_back(dest) ;
Thomas's avatar
Thomas committed
857 858 859
		}
	}
}
Pierre Kraemer's avatar
Pierre Kraemer committed
860
#endif
Thomas's avatar
Thomas committed
861

Pierre Kraemer's avatar
Pierre Kraemer committed
862 863
bool Simulator::importAgents(std::string filename)
{
David Cazier's avatar
David Cazier committed
864
	std::ifstream myfile(filename.c_str(), std::ios::in) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
865

David Cazier's avatar
David Cazier committed
866 867
	if (!myfile.good())
	{
David Cazier's avatar
David Cazier committed
868 869
		std::cerr << "(export) Unable to open file " << filename << std::endl ;
		return false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
870 871
	}

David Cazier's avatar
David Cazier committed
872 873 874
	std::string line, token ;
	while (myfile.good())
	{
David Cazier's avatar
David Cazier committed
875
		getline(myfile, line) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
876

David Cazier's avatar
David Cazier committed
877 878
		if (line.size() > 1)
		{
David Cazier's avatar
David Cazier committed
879
			std::istringstream iss(line) ;
David Cazier's avatar
David Cazier committed
880
			float x, y, z ;
Pierre Kraemer's avatar
Pierre Kraemer committed
881

David Cazier's avatar
David Cazier committed
882 883 884
			iss >> x ;
			iss >> y ;
			iss >> z ;
Pierre Kraemer's avatar
Pierre Kraemer committed
885

886
			VEC3 pos(x, y, z) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
887 888 889
#ifdef SPATIAL_HASHING
			Agent* a = new Agent(this, pos) ;
			agents_.push_back(a) ;
pitiot's avatar
maj  
pitiot committed
890
			envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
891
#else
892 893
			Dart dCell = envMap_.getBelongingCell(pos) ;
			agents_.push_back(new Agent(this, pos, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
894
#endif
895
			agents_.back()->goals_.push_back(pos) ;
David Cazier's avatar
David Cazier committed
896
			agents_.back()->goals_.push_back(-1.0f * pos) ;
897
			agents_.back()->curGoal_ = 1 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
898 899 900
		}
	}

David Cazier's avatar
David Cazier committed
901
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
902

David Cazier's avatar
David Cazier committed
903 904
	myfile.close() ;
	return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
905 906 907 908
}

bool Simulator::exportAgents(std::string filename)
{
David Cazier's avatar
David Cazier committed
909
	std::ofstream out(filename.c_str(), std::ios::out) ;
David Cazier's avatar
David Cazier committed
910 911
	if (!out.good())
	{
David Cazier's avatar
David Cazier committed
912 913
		std::cerr << "(export) Unable to open file " << filename << std::endl ;
		return false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
914 915
	}

pitiot's avatar
maj  
pitiot committed
916
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
917
#ifdef SPATIAL_HASHING