simulator.cpp

#include "simulator.h"

Simulator::Simulator(unsigned int config, unsigned int minS, unsigned int nbAgent, unsigned int nbObst) :
	timeStep_(0.05f),
	globalTime_(0.0f),
	nbSteps_(0),
	nbUpdates(0),
	nbSorts(0),
	nbRefineCandidate(0),
	nbCoarsenCandidate(0),
	nearNeighbors(0),
	totalNeighbors(0),
	avoidance(1),
	nb_dead(0)
{
	minSize=minS;
	multires=true;
	detect_agent_collision=true;
	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 :
//			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 ;
		case 5:
			envMap_.init(config, 1600.0f, 1200.0f, minSize, 400.0f) ; //svg import
			setupScenario(nbAgent) ;
			addPathsToAgents();
//			SelectorCellNotMarked<PFP::MAP> scnm(envMap_.pedWayMark);
			addMovingObstacles(nbObst);
//			addPathToObstacles();
			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

	for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i)
	{
		movingObstacles_[i]->computePrefVelocity() ;
		movingObstacles_[i]->computeNewVelocity() ;
#ifndef SPATIAL_HASHING
		envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
#endif
		movingObstacles_[i]->update() ;

		//PIERRE
	}

	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	{
		if (agents_[i]->alive)
		{
			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() ;
		}
	}

#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 ;
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	{
		if (agents_[i]->alive)
		{
			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)
			{
//			case CGoGN::Algo::MovingObjects::CROSS_EDGE :
//				envMap_.agentChangeFaceThroughEdge(agents_[i]);
//				break;
//			case CGoGN::Algo::MovingObjects::CROSS_OTHER :
				envMap_.agentChangeFace(agents_[i], oldFace) ;
//				break;
			}
		}
		else
			nb_dead++ ;

	}

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

	 if (multires)
		 envMap_.updateMap() ;
#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, 1600.0f, 960.0f, minSize, 400.0f) ; //cases fines
	}

	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 ;
		addAgent(start, goal) ;
	}
	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;
	for (unsigned int i = 0 ; i < nbObstacles ; i++)
	{
		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/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];
//		std::vector<VEC3> goals;
//		for(unsigned int k = 0 ; k < nbObstacles/2 ; k++)
//		{
//			goals.push_back(positions[(i+k)%(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) * (2*radius/3), std::sin(2.0f*M_PI*k/nbGoals) * (2*radius/3), 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);

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

		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)
{
	if (multires)
	{
		envMap_.init(config, 1600.0f, 960.0f, minSize, 320.0f) ; //grosses cases
	}
	else
	{

		envMap_.init(config, 1600.0f, 960.0f, minSize, 320.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) / 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
		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) / 4 ;
	xStartDelta = envMap_.geometry.size(0) / 2 ;
	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 ;

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


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

	// Arrivée des obstacles à l'opposée

	yGoalDelta = 3* 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]);
	}
}

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)
{
	/*
	 * 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() ;

	unsigned int nbx = 2 ;
	unsigned int nby = 2 ;

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

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

		if (found)
		{
			float ecart = 3.0f ;
			VEC3 posinit = VEC3(pos[0] - (float(nbx) / 2.0f * ecart),
			                    pos[1] - (float(nby) / 2.0f * ecart), pos[2]) ;
			for (unsigned int curx = 0 ; curx < nbx ; ++curx)
			{
				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::addMovingObstacles(unsigned int nb)
{
	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::Geometry::convexFaceArea<PFP>(envMap_.map, d, envMap_.position);
				if(area>1400)
				{
					dCell = d ;
					found = true ;
				}
			}
			d = tF.next() ;
		}

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

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

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

			vPos.push_back(start-xSide+ySide);
			vPos.push_back(start-xSide-ySide);
			vPos.push_back(start+xSide-ySide);
			vPos.push_back(start+xSide+ySide);
		}
			break;
		case 1 :
		{
			mm = new MovingMesh(envMap_, d, "meshRessources/Limace.ply");
			movingMeshes_.push_back(mm);
			vPos = mm->computeProjectedPointSet(maxHeight);
		}
			break;
		default:
			std::cout << "Unknown obstacle type" << std::endl;
			return;
	}

	bool orientAccordingToFace=true;
	if(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<PFP::MAP> tv(mm->map);
		for(Dart d = tv.begin() ; d != tv.end() ; d = tv.next())
		{
			mm->position[d]+=rotate(mm->position[d], bary, angle);
		}
		vPos = mm->computeProjectedPointSet(maxHeight);
	}

	std::vector<VEC3> goals;
	goals.push_back(start);
	MovingObstacle * mo = new MovingObstacle(this, movingObstacles_.size(),vPos,goals, (obstType==0), false);

	movingObstacles_.push_back(mo);

	if(obstType==1)
	{
		//PIERRE : calcul coord bary d som du movMesh en fction de la cage movingObstacle
	}
}

void Simulator::addPathToObstacles()
{
	unsigned int dartDistForPath = 50 ;

	for (std::vector<MovingObstacle*>::iterator it = movingObstacles_.begin() ; it != movingObstacles_.end() ; ++it)
	{
		Dart dStart = (*it)->registering_part->d;

		while(envMap_.pedWayMark.isMarked(dStart))
		{
			if(!envMap_.pedWayMark.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)
			|| envMap_.pedWayMark.isMarked(dStop)
			; ++j)
		{
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end())
				dStop = envMap_.map.begin() ;
		}

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

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

	for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
	{
		VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
		dest /= 2.0f ;

		mo->goals_.push_back(dest) ;
	}

	path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
												envMap_.position,
												dGoal,
												dStart,
												envMap_.pedWayMark) ;

	for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
	{
		VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
		dest /= 2.0f ;

		mo->goals_.push_back(dest) ;
	}

//	if(((mo->front-mo->center)*(mo->goals_[mo->curGoal_]-mo->center))<0)
//	{
//		std::cout << __FILE__ << " " << __LINE__ << " rotate limace" << std::endl;
//
//		Geom::Matrix44f m;
//		m.identity();
//		rotateZ(180.0f,m);
//
//		TraversorV<PFP::MAP> tv(mo->map);
//		for(Dart d = tv.begin() ; d != tv.end() ; d = tv.next())
//		{
//			mo->position[d] -= mo->center;
//			mo->position[d] = Geom::transform(mo->position[d],m);
//			mo->position[d] += mo->center;
//			mo->position[d][2] += 10.0f;
//		}
//	}
}

#ifndef SPATIAL_HASHING
void Simulator::addPathToCorner()
{
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	{
		agents_[i]->goals_.clear() ;
		agents_.back()->curGoal_ = 1 ;

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

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

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

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

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

void Simulator::addPathsToAgents()
{
	//city
	unsigned int dartDistForPath = 50 ;
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	{
		agents_[i]->goals_.clear() ;

		TraversorF<PFP::MAP> tF(envMap_.map);

		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
		for (unsigned int j = 0 ;
			/*!envMap_.pedWayMark.isMarked(dStop) || */
			envMap_.buildingMark.isMarked(dStop)
		    || j < dartDistForPath + rand() * 20
		    || envMap_.map.sameFace(dStop, dStart)
		    || envMap_.obstacleMark.isMarked(dStop)
		    ; ++j)
		{
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end())
				dStop = envMap_.map.begin() ;
		}

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

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

		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
		{
			Dart d = *it;
			while(envMap_.map.isBoundaryEdge(d))
			{
				d = envMap_.map.phi1(d);
			}

//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);

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

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

		Dart dStop2 = dStop ;
		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)
		    || envMap_.obstacleMark.isMarked(dStop)
		    ; ++j)
		{
			envMap_.map.next(dStop2) ;
			if (dStop2 == envMap_.map.end())
				dStop2 = envMap_.map.begin() ;
		}

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

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

		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
		{
			Dart d = *it;
			while(envMap_.map.isBoundaryEdge(d))
			{
				d = envMap_.map.phi1(d);
			}

//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
			VEC3 dest = envMap_.position[d] + envMap_.position[envMap_.map.phi1(d)] ;
			dest /= 2.0f ;
			dest[2] = 0 ;

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

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

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

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

			dest[2] = 0 ;

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

void Simulator::addPathsToAgents_height()
{
	//city
	unsigned int dartDistForPath = 50 ;
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	{
		agents_[i]->goals_.clear() ;

		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
		for (unsigned int j = 0 ;
		    envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath + rand() * 20
		        || envMap_.map.sameFace(dStop, dStart) ; ++j)
		{
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end()) dStop = envMap_.map.begin() ;
		}

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

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

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

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

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

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

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

bool Simulator::importAgents(std::string filename)