simulator.cpp

#include "simulator.h"
#include "env_generator.h"

timespec timespec_delta(const timespec& t1, const timespec& t2) {
    timespec result;

    if ((t2.tv_nsec - t1.tv_nsec) < 0 /* ns */) {
        result.tv_sec = t2.tv_sec - t1.tv_sec - 1;
        result.tv_nsec = 1000000000 /* ns */ + t2.tv_nsec - t1.tv_nsec;
    } else {
        result.tv_sec = t2.tv_sec - t1.tv_sec;
        result.tv_nsec = t2.tv_nsec - t1.tv_nsec;
    }

    return result;
}

Simulator::Simulator(unsigned int config, unsigned int minS, unsigned int nbAgent, unsigned int nbObst, bool resolution) :
	timeStep_(0.025f),
//	timeStep_(0.2f),
	globalTime_(0.0f),
	nbSteps_(0),
	nbUpdates(0),
	nbSorts(0),
	nbRefineCandidate(0),
	nbCoarsenCandidate(0),
	nearNeighbors(0),
	totalNeighbors(0),
	avoidance(1),
	nb_dead(0),
	time_agent(0),
	time_obstacle(0),
	time_behave(0),
	time_multires(0)
{
	minSize=minS;
	multires=resolution;
	detect_agent_collision=false;
	srand(10) ;
	nbStepsPerUnit_ = 1 / timeStep_ ;
	this->config=config;
	init(2.0f, nbAgent, nbObst) ;
}

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

void Simulator::init( float dimension, unsigned int nbAgent, unsigned int nbObst, bool enablePathFinding)
{
	std::cout << "Setup scenario" << std::endl ;

	switch (config)
	{
		case 0 :
			setupCircleScenario(nbAgent,nbObst) ;
//			addPathsToAgents();
			break ;
		case 1 :
			setupCorridorScenario(nbAgent,nbObst) ;
			break ;
		case 2 :
			setupSnakeCorridorScenario(nbAgent,nbObst,10) ;
			break ;
		case 3 :
			envMap_.init(config, 1000.0f, 1000.0f, minSize, 100.0f) ; //grosses cases
			setupScenario(nbAgent, false) ;
			addMovingObstacles(nbObst, 1);
			addPathToObstacles(envMap_.buildingMark, true);
////			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 ;
		case 5:
			envMap_.init(config, 1600.0f, 1200.0f, minSize, 400.0f) ; //svg import
			setupScenario(nbAgent, true) ;
//			SelectorCellNotMarked<PFP::MAP> scnm(envMap_.pedWayMark);
			addMovingObstacles(nbObst, 1);
			addPathToObstacles(envMap_.pedWayMark, false);
			addPathsToAgents();
			break;
		case 6:
#ifdef TWO_AND_HALF_DIM
			envMap_.init(config,200.0,200.0, minSize, 400.0f);
			setupPlanetScenario(nbAgent,nbObst);
			addMovingObstacles(nbObst, 0);
			addPathToObstacles(envMap_.buildingMark, true);
#else
			std::cout << "Agents not in 2.5D mode" << std::endl;
#endif
			break;
		default:
			std::cout << "Unknown scenario !" << std::endl ;
			exit(1) ;
	}

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

#ifndef SPATIAL_HASHING
	if (enablePathFinding)
	{
		if (dimension == 2.0f)
			addPathsToAgents() ;
		else if (dimension == 2.5f)
			addPathsToAgents_height() ;
	}
#endif

	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	{
		agents_[i]->updateObstacleNeighbors() ;
		agents_[i]->updateAgentNeighbors() ;
	}

#ifndef SPATIAL_HASHING
	if (multires)
		envMap_.subdivideToProperLevel() ;
	//	envMap_.subdivideAllToMaxLevel();
#endif
}

void Simulator::doStep()
{
#ifndef SPATIAL_HASHING
	envMap_.clearUpdateCandidates() ;
	envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
#endif
	struct timespec begTime, endTime ;

	for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i)

	{
//		unsigned int i =12;
		clock_gettime(CLOCK_MONOTONIC, &begTime) ;
		movingObstacles_[i]->computePrefVelocity() ;
		movingObstacles_[i]->computeNewVelocity() ;
		movingObstacles_[i]->initForces();
		clock_gettime(CLOCK_MONOTONIC, &endTime) ;
		time_behave+= timespec_delta(begTime,endTime).tv_nsec;
//		CGoGNout<<" Obstacle "<<i<<" : ";
//		for(unsigned int j=0;j<movingObstacles_[i]->nbVertices;j++)
//		{
//			VEC3 dist =movingObstacles_[i]->parts_[j]->getPosition()-movingObstacles_[i]->parts_[(j+1)%movingObstacles_[i]->nbVertices]->getPosition();
//			CGoGNout<<dist.norm()<<" || ";
//		}
//		CGoGNout<<CGoGNendl;

	}

	clock_gettime(CLOCK_MONOTONIC, &begTime) ;
	for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i)
	{
//		unsigned int i =12;
		movingObstacles_[i]->updateAgentNeighbors();
		movingObstacles_[i]->updateObstacleNeighbors();
		movingObstacles_[i]->updateForces() ;

	}
	clock_gettime(CLOCK_MONOTONIC, &endTime) ;
	time_obstacle+= timespec_delta(begTime,endTime).tv_nsec;
//	CGoGNout<<"deplacement obstacles"<<CGoGNendl;
	for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i)
	{
//		unsigned int i =12;
		clock_gettime(CLOCK_MONOTONIC, &begTime) ;
		movingObstacles_[i]->applyForces();
		clock_gettime(CLOCK_MONOTONIC, &endTime) ;
		time_obstacle+= timespec_delta(begTime,endTime).tv_nsec;
		movingObstacles_[i]->updateMesh() ;
	}
//	CGoGNout<<"deplacement obstacles fin"<<CGoGNendl;

	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	{
		if (agents_[i]->alive)
		{
			clock_gettime(CLOCK_MONOTONIC, &begTime) ;
			agents_[i]->updateObstacleNeighbors() ;

#ifndef SPATIAL_HASHING
			envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
#endif
			agents_[i]->updateAgentNeighbors() ;
#ifndef SPATIAL_HASHING
			envMap_.map.setCurrentLevel(0) ;
#endif
			clock_gettime(CLOCK_MONOTONIC, &endTime) ;
			time_agent+=timespec_delta(begTime,endTime).tv_nsec;
			clock_gettime(CLOCK_MONOTONIC, &begTime) ;
			agents_[i]->computePrefVelocity() ;
			agents_[i]->computeNewVelocity() ;
			clock_gettime(CLOCK_MONOTONIC, &endTime) ;
			time_behave+=timespec_delta(begTime,endTime).tv_nsec;
		}
	}

#ifdef SPATIAL_HASHING
	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

	if (multires)
		envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;

	nb_dead = 0 ;
	clock_gettime(CLOCK_MONOTONIC, &begTime) ;
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	{
		if (agents_[i]->alive)
		{
			Dart oldFace = agents_[i]->part_.d ;
			agents_[i]->update() ;

			if (agents_[i]->part_.crossCell != CGoGN::Algo::Surface::MovingObjects::NO_CROSS)
				envMap_.agentChangeFace(agents_[i], oldFace) ;
		}
		else
			nb_dead++ ;

	}
	clock_gettime(CLOCK_MONOTONIC, &endTime) ;
	time_agent+= timespec_delta(begTime,endTime).tv_nsec;

	nbRefineCandidate += envMap_.refineCandidate.size() ;
	nbCoarsenCandidate += envMap_.coarsenCandidate.size() ;

	 if (multires)
	 {
		 clock_gettime(CLOCK_MONOTONIC, &begTime) ;
		 envMap_.updateMap() ;
		 clock_gettime(CLOCK_MONOTONIC, &endTime) ;
	 	 time_multires+= timespec_delta(begTime,endTime).tv_nsec;
	 }
#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 ;
}

void Simulator::addAgent(const VEC3& start, const VEC3& goal)
{
	agents_.push_back(new Agent(this, start, goal)) ;
}

void Simulator::addAgent(const VEC3& start, const VEC3& goal, Dart d)
{
	agents_.push_back(new Agent(this, start, goal, d)) ;
}

void Simulator::setupCircleScenario(unsigned int nbAgents , unsigned int nbObstacles)
{
	if (multires)
		envMap_.init(config, 2000.0f, 2000.0f, minSize, 400.0f) ; //grosses cases
	else

	{

//		envMap_.init(config, 2000.0f, 2000.0f, minSize, 50.0f) ; //cases fines
		envMap_.init(config, 2000.0f, 2000.0f, minSize, 400.0f) ; //grosses cases
	}


	std::cout << " - Setup Circle 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 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 ;
//	int radius = nbAgents/(Agent::radius_*Agent::radius_);
	std::cout << "radius " << radius << std::endl;
	VEC3 center = (envMap_.geometry.min() + envMap_.geometry.max()) / 2 ;

	for (unsigned int i = 0 ; i < nbAgents ; ++i)
	{
		double angle = i * 2.0f * M_PI / float(nbAgents) ;
		VEC3 v(std::cos(angle) * radius, std::sin(angle) * radius, 0) ;
		VEC3 start = center + v ;
		VEC3 goal = center - v ;
		addAgent(start, goal) ;
//		CGoGNout<<"agent n°" << agents_[i]->agentNo << " : "<< agents_[i]<<CGoGNendl;
	}
	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;
	int curGoal=0;

	for (unsigned int i = 0 ; i < nbObstacles ; i++)
	{
		double angle = nbObstacles<2 ? 0 : i * 2.0f * M_PI / float((float)nbObstacles/2.0f) ;
		//v : position on the circle, start position on the circle relative to the scene
		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/2), std::sin(angle) * (radius/2), 0) ;
		}
		start = center + v ;
		positions.push_back(start);
	}

	for (unsigned int i = 0 ; i < nbObstacles/2 ; i++)
	{
		VEC3 start = positions[i];

		unsigned int nbGoals=360;
		std::vector<VEC3> goals;
		for(unsigned int k = 0 ; k < nbGoals ; k++)
		{
			goals.push_back(VEC3(std::cos(2.0f*M_PI*k/nbGoals) * (2*radius/3), std::sin(2.0f*M_PI*k/nbGoals) * (2*radius/3), 0)) ;
		}

		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);
		curGoal = ((int)(i*float(nbGoals)/float(nbObstacles/2.0f))+1)%nbGoals;
		mo4= new MovingObstacle(this, i,vPos,goals, true, true,curGoal);

//		mo4= new MovingObstacle(this, i,vPos,goals, true, true);
//		mo4->curGoal_ = i*float(nbGoals)/float(nbObstacles/2.0f);

		movingObstacles_.push_back(mo4);
	}

	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]);
//		}

		unsigned int nbGoals=360;
		std::vector<VEC3> goals;
		for(unsigned int k = 0 ; k < nbGoals ; k++)
		{
			goals.push_back(VEC3(std::cos(2.0f*M_PI*k/nbGoals) * (radius/2), std::sin(2.0f*M_PI*k/nbGoals) * (radius/2), 0)) ;
		}

		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);
		curGoal = ((int)(i*float(nbGoals)/float(nbObstacles/2.0f))+5)%nbGoals;
		mo4= new MovingObstacle(this, i,vPos,goals, true, true,curGoal);

//		mo4= new MovingObstacle(this, i,vPos,goals, true, true);
//		mo4->curGoal_ = (i-nbObstacles/2)*float(nbGoals)/float(nbObstacles/2.0f);

		movingObstacles_.push_back(mo4);
	}

#ifndef SPATIAL_HASHING
	envMap_.clearUpdateCandidates() ;
	envMap_.map.setCurrentLevel(0) ;
#endif

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

void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles)
{
	VEC3 xSide (5.0f,0.0f,0.0f);
	VEC3 ySide (0.0f,10.0f,0.0f);
	if (multires)
	{
//		envMap_.init(config, 1600.0f, 960.0f, minSize, 320.0f) ; //grosses cases
		envMap_.init(config, nbObstacles > 60 ? ((nbObstacles*20*xSide[0])/6) : 1000.0f , 960.0f, minSize, 320.0f) ; //grosses cases
	}
	else
	{

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


	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) / 10 ;
	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) / 10 ;
	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
		if (i % 2 == 1)
			addAgent(start, goal) ;
		else
			addAgent(goal, start) ;
	}

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

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


	std::vector<VEC3> vPos;
	MovingObstacle* mo4;

	for (unsigned int i = 0 ; i < nbObstacles ; i++)
	{
		float x = xStartMin + ((int)(i*2*xSide[0])) % xStartDelta;
//		std::cout << "x ? " << x << " xStartDelta " << xStartDelta << std::endl;
		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);
		}
		std::vector<VEC3> goals;
		goals.push_back(start);
		goals.push_back(goal);

		mo4= new MovingObstacle(this, i,vPos,goals, true, false,1);


//		//for generating a random path
//		unsigned int dartDistForPath = 50 ;
//		mo4->goals_.clear() ;
//		Dart dStart = mo4->registering_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() ;
//		}
//
//		addPathToObstacle(mo4, dStart, dStop);
//		addPathToObstacle(mo4, dStop, dStart);

		movingObstacles_.push_back(mo4);
	}
}

void Simulator::setupSnakeCorridorScenario(unsigned int nbAgents, unsigned int nbSnakes, int snakeSize)
{
	if (multires)
		envMap_.init(config, 1600.0f, 960.0f, minSize, 160.0f) ; //grosses cases
	else
		envMap_.init(config, 1600.0f, 960.0f, minSize, 160.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 ;
		}

		addAgent(start, goal) ;

	}

	// 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) / 10 ;

	// 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::cout << "geom : " << envMap_.geometry << std::endl;

	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<80)
		{
			x = xStartMin + rand() % xStartDelta;
			goal=VEC3 (x, yGoalMin  + rand() % yGoalDelta,  0) ;


			if ((r==0 || angle(goal-goals[r],goals[r]-goals[r-1])<M_PI/2.0f) )
			{

				std::cout << "new goal " << goal << std::endl;

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

void Simulator::setupCityScenario(int nbLines, int nbRank)
{
	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 ;

	for (int i = 0 ; i < nbLines ; ++i)
	{
		for (int j = 0 ; j < nbRank ; ++j)
		{
			VEC3 posagent(xCornerDown + i * 10.0f, yCornerDown + j * 10.0f, 0.0f) ;
			addAgent(posagent, -1.0f * posagent) ;
		}
	}
}

void Simulator::setupScenario(unsigned int nbMaxAgent, bool pedWay)
{
	/*
	 * Add agents, specifying their start position, and store their goal on the
	 * opposite side of the environment.
	 */
	TraversorF<PFP::MAP> tF(envMap_.map);
	Dart d = tF.begin() ;

//-----------------------
#define SCENARIO3
#if defined SCENARIO3
	unsigned int nbx = 5 ;
	unsigned int nby = 5 ;
#elif defined SCENARIO5
	unsigned int nbx = 2 ;
	unsigned int nby = 2 ;
#else
	unsigned int nbx = 1 ;
	unsigned int nby = 1 ;
#endif

//-----------------------

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

	for (unsigned int i = 0 ; i < bMax && d != envMap_.map.end() ; ++i)
	{
		VEC3 pos;
		bool found = false ;
		Dart dCell ;
		while (!found && d != tF.end())
		{
			if (!envMap_.buildingMark.isMarked(d)
			    && (!pedWay || envMap_.pedWayMark.isMarked(d))
			    )
			{
				pos = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
				pos[2] = 0 ;
				dCell = d ;
				found = true ;
			}
			d = tF.next() ;
		}

		if (found)
		{
			float ecart = 2.0f*Agent::radius_;
			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)
			{
				for (unsigned int cury = 0 ; cury < nby ; ++cury)
				{
					VEC3 posagent = posinit + VEC3(ecart * curx, ecart * cury, 0.0f) ;
					addAgent(posagent, -1.0f * posagent, dCell) ;
				}
			}
		}
	}
	std::cout << "nb agents : " << agents_.size() << std::endl ;
	swapAgentsGoals() ;
}

void Simulator::setupPlanetScenario(unsigned int nbAgents, unsigned int nbObstacles)
{
	/*
	* Add agents, specifying their start position, and store their goals on the
	* opposite side of the environment.
	*/
	Dart d = envMap_.map.begin();
	CellMarker<FACE> filled(envMap_.map);

	unsigned int nbx = 1;
	unsigned int nby = 1;

	unsigned int bMax = nbAgents / (nbx*nby);

	for (unsigned int i = 0; i < bMax && d != envMap_.map.end(); ++i)
	{
		bool found = false;
		VEC3 pos;
		Dart dCell;
		while(!found && d != envMap_.map.end())
		{
			if(!filled.isMarked(d) && !envMap_.buildingMark.isMarked(d))
			{
				filled.mark(d);
				pos = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position);
				dCell = d;
				found = true;
			}
			envMap_.map.next(d);
		}

		if(found)
		{
			float ecart = 3.0f;
			Geom::Plane3D<PFP::REAL> pl(Algo::Surface::Geometry::faceNormal<PFP>(envMap_.map, dCell, envMap_.position), 0.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)
			{
				for(unsigned int cury = 0; cury < nby; ++cury)
				{
					VEC3 displ(ecart * curx, ecart * cury, 0.0f);
					Agent::rotate(Agent::xyPlane, pl.normal(), displ);
					VEC3 posagent = posinit + displ;
//					VEC3 posagent = posinit + VEC3(ecart * curx, ecart * cury, 0.0f);
					addAgent(posagent,-1.0f * posagent, dCell);
//					agents_.back()->goals_.push_back(posagent);
//					agents_.back()->goals_.push_back(VEC3(0,0,0));
//					agents_.back()->goals_.push_back(-1.0f * posagent);
					agents_.back()->curGoal_ = 1;
				}
			}
		}
	}
	std::cout << "nb agents : " << agents_.size() << std::endl;

	swapAgentsGoals();
}

void Simulator::addMovingObstacles(unsigned int nb, unsigned int type)
{
	TraversorF<PFP::MAP> tF(envMap_.map);
	Dart d = tF.begin() ;
	for (unsigned int i = 0 ; i < nb && d != envMap_.map.end() ; ++i)
	{
		bool found = false ;
		Dart dCell ;
		while (!found && d != tF.end())
		{
			if (!envMap_.buildingMark.isMarked(d)
				&& !envMap_.pedWayMark.isMarked(d)
				)
			{
				float area = Algo::Surface::Geometry::convexFaceArea<PFP>(envMap_.map, d, envMap_.position);
				if(area>1400)
				{
					dCell = d ;
					found = true ;
				}
			}
			d = tF.next() ;
		}

		if (found)
			addMovingObstacle(dCell,type) ;
	}
}

void Simulator::addMovingObstacle(Dart d, unsigned int obstType)
{
	std::vector<VEC3> vPos;
	VEC3 start;
	float maxHeight=2.0f;

	MovingMesh* mm = NULL;

	switch(obstType)
	{
		case 0 :
		{
			start = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
			VEC3 xSide (2.0f,0.0f,0.0f);
			VEC3 ySide (0.0f,5.0f,0.0f);

#ifdef TWO_AND_HALF_DIM
			float xSideF=2.0f;
			float ySideF=5.0f;
			Geom::Plane3D<PFP::REAL> pl(Algo::Surface::Geometry::facePlane<PFP>(envMap_.map, d, envMap_.position));
			VEC3 norm =Algo::Surface::Geometry::faceNormal<PFP>(envMap_.map, d, envMap_.position) ;
			VEC3 dir1 =VEC3(norm[1]+norm[2],norm[2]-norm[0],-norm[0]-norm[1]);

			dir1.normalize();
			VEC3 dir2 = norm ^dir1;

			vPos.push_back(start-xSideF*dir1-ySideF*dir2);
			vPos.push_back(start+xSideF*dir1-ySideF*dir2);
			vPos.push_back(start+xSideF*dir1+ySideF*dir2);
			vPos.push_back(start-xSideF*dir1+ySideF*dir2);
#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);
#endif

		}
			break;
		case 1 :
		{
//			mm = new MovingMesh(envMap_, d, "./meshRessources/scenario3/limace_occ.obj");
			mm = new MovingMesh(envMap_, d, "./meshRessources/LimaceLvl1.obj");
//			mm = new MovingMesh(envMap_, d, "./meshRessources/cylinder.obj");
//			mm = new MovingMesh(envMap_, d, "./meshRessources/simpleCyl.obj");
			movingMeshes_.push_back(mm);
			vPos = mm->computeProjectedPointSet(maxHeight);

			//scale projected pointset
			VEC3 bary(0);
			for(std::vector<VEC3>::iterator it = vPos.begin() ; it != vPos.end() ; ++it)
				bary += *it;
			bary /= vPos.size();

			float scale = 1.2f;
			for(std::vector<VEC3>::iterator it = vPos.begin() ; it != vPos.end() ; ++it)
				*it = *it*scale - bary*(scale-1.0f);
		}
			break;
		default:
			std::cout << "Unknown obstacle type" << std::endl;
			return;
	}

	bool orientAccordingToFace=false;
	if(mm!=NULL && orientAccordingToFace)
	{
		while(envMap_.pedWayMark.isMarked(envMap_.map.phi2(d)))
		{
			d = envMap_.map.phi1(d);
		}

		VEC3 mid = (envMap_.position[d]+envMap_.position[envMap_.map.phi1(d)])*0.5f;
		VEC3 front = vPos[0];

		VEC3 bary(0);
		for(std::vector<VEC3>::iterator it = vPos.begin() ; it != vPos.end() ; ++it)
			bary += *it;

		bary /= vPos.size();

		float angle = get_angle(mid - bary,front  - bary);

		TraversorV<PFP2::MAP> tv(mm->map);
		for(Dart d = tv.begin() ; d != tv.end() ; d = tv.next())
		{
			mm->position[d] += rotate2D(mm->position[d], bary, angle);
		}

//		for(std::vector<VEC3>::iterator it = vPos.begin() ; it != vPos.end() ; ++it)
//			*it += rotate2D(*it , bary, angle);
	}

	std::vector<VEC3> goals;
	goals.push_back(start);

	MovingObstacle * mo = new MovingObstacle(this, movingObstacles_.size(),vPos,goals, (obstType==0), (obstType==0));

	movingObstacles_.push_back(mo);

	if(mm != NULL)
	{
		mo->attachMesh(mm);
//		std::cout << "here you go" << std::endl;
//		addAgent(vPos[0], vPos[0], d);
//		mo->attachAgent(agents_.back());
	}
}

void Simulator::addPathToObstacles(CellMarker<FACE>& markForbid, bool goalCenterCell)
{
	unsigned int dartDistForPath = 50 ;

	for (std::vector<MovingObstacle*>::iterator it = movingObstacles_.begin() ; it != movingObstacles_.end() ; ++it)
	{
		(*it)->goals_.clear();

		Dart dStart = (*it)->parts_[0]->d;

		while(markForbid.isMarked(dStart))
		{
			if(!markForbid.isMarked(envMap_.map.phi2(dStart)))
				dStart = envMap_.map.phi2(dStart);
			else
				dStart = envMap_.map.phi1(dStart);
		}

		Dart dStop = dStart;

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

		addPathToObstacle(*it, dStart, dStop, markForbid, goalCenterCell);
	}
}

void Simulator::addPathToObstacle(MovingObstacle * mo, Dart dStart, Dart dGoal, CellMarker<FACE>& markForbid, bool goalCenterCell)
{
	std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
																   envMap_.position, dStart,
																   dGoal,
																   markForbid) ;