env_map.cpp

#include "env_map.h"
#include "utils.h"
#include "agent.h"
#include "obstacle.h"

#include "Geometry/inclusion.h"
#include "Algo/MovingObjects/particle_cell_2D_memo.h"
#include "Algo/Modelisation/subdivision.h"
#include "Algo/Geometry/normal.h"
#include "Algo/Import/importSvg.h"
#include "Algo/BooleanOperator/mergeVertices.h"

#include "env_generator.h"

using namespace CGoGN ;

EnvMap::EnvMap() :
	obstacleDistance(Agent::range_),
	obstacleMarkS(mapScenary),
	buildingMarkS(mapScenary),
	obstacleMark(map),
	buildingMark(map),
	pedWayMark(map),

#ifndef SPATIAL_HASHING
	refineMark(map),
	coarsenMark(map)
#else
	ht_agents(1024),
	ht_neighbor_agents(1024),
	ht_obstacles(1024)
#endif
{
	position = map.addAttribute<VEC3, VERTEX>("position") ;
	normal = map.addAttribute<VEC3, VERTEX>("normal") ;

#ifndef SPATIAL_HASHING
	agentvect = map.addAttribute<PFP::AGENTVECT, FACE>("agents") ;
	neighborAgentvect = map.addAttribute<PFP::AGENTVECT, FACE>("neighborAgents") ;
	obstvect = map.addAttribute<PFP::OBSTACLEVECT, FACE>("obstacles") ;
	neighborObstvect = map.addAttribute<PFP::OBSTACLEVECT, FACE>("neighborObstacles") ;
	subdivisableFace = map.addAttribute<PFP::BOOLATTRIB, FACE>("subdivisableFace") ;

	refineCandidate.reserve(100) ;
	coarsenCandidate.reserve(100) ;
#endif
}

void EnvMap::init(unsigned int config, REAL width, REAL height, REAL minSize, REAL maxSize)
{
	std::cout << "Init EnvMap" << std::endl ;
	VEC3 bottomLeft(-width / 2, -height / 2, 0.0f) ;
	VEC3 topRight(width / 2, height / 2, 0.0f) ;

	geometry.reset() ;
	geometry.addPoint(bottomLeft) ;
	geometry.addPoint(topRight) ;
	minCellSize = minSize ;
	maxCellSize = maxSize ;
	std::cout << " - Geometry : " << geometry ;
	std::cout << " - Cell size between : " << minSize << " and " << maxSize << std::endl ;
#ifdef SPATIAL_HASHING
	std::cout << " - Table de hachage : " << agentGridSize(0) << " x " << agentGridSize(1) << std::endl ;
#endif

	switch (config)
	{
		case 0 :
			CityGenerator::generateGrid<PFP>(*this) ;
			break ;
		case 1 :
			CityGenerator::generateGrid<PFP>(*this) ;
			break ;
		case 2 :
			CityGenerator::generateCity<PFP>(*this) ;
			// CityGenerator::generateMall<PFP>(map, position, obstacleMark, buildingMark, sideSize);
			break ;
		case 3 : {
//			std::string filename = "/home/jund/Desktop/drawingQuads.svg" ;
//			std::string filename = "/home/jund/Desktop/drawingTest.svg" ;
//			std::string filename = "/home/jund/Desktop/drawingSimple.svg" ;
//			std::string filename = "/home/jund/Desktop/drawingLines.svg" ;
//			std::string filename = "/home/jund/Desktop/mapKrutSimple.svg" ;
//			std::string filename = "/home/jund/Desktop/mapKrut.svg" ;
//			Algo::Import::importSVG<PFP>(map, filename, position, obstacleMark, buildingMark) ;

//			Algo::BooleanOperator::mergeVertices<PFP>(map, position) ;
//			map.closeMap() ;
//			Algo::Modelisation::CatmullClarkSubdivision<PFP>(map, position) ;
//			Algo::Modelisation::computeDual<PFP>(map) ;
		}
			CityGenerator::generateCity<PFP>(*this) ;
			break ;
		case 4 :
			CityGenerator::generatePlanet<PFP>(*this) ;
			break ;
	}

//	CityGenerator::simplifyFreeSpace<PFP>(map, position, obstacleMark, buildingMark);
//	CityGenerator::convexifyFreeSpace<PFP>(map, position, obstacleMark, buildingMark);
//	CityGenerator::installGuardRail<PFP>(map, position, obstacleMark, buildingMark, 5.0f);

#ifndef SPATIAL_HASHING
	map.init() ;
//	registerObstaclesInFaces();
	// TODO Check registerWallInFaces();
	registerWallInFaces() ;
//	subdivideAllToMaxLevel();

	for (unsigned int i = subdivisableFace.begin(); i < subdivisableFace.end(); subdivisableFace.next(i))
		subdivisableFace[i].first = false ;
#endif

}

void EnvMap::markPedWay()
{
	CellMarker<FACE> treat(map) ;
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!treat.isMarked(d))
		{
			treat.mark(d) ;

			Dart dd = d ;
			do
			{
				Dart ddd = dd ;
				do
				{
					if (obstacleMark.isMarked(dd))
					{
						pedWayMark.mark(d) ;
						break ;
					}
					ddd = map.alpha1(ddd) ;
				} while (ddd != dd) ;

				dd = map.phi1(dd) ;

			} while (dd != d) ;
		}
	}

	treat.unmarkAll() ;
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!treat.isMarked(d))
		{
			treat.mark(d) ;

			Dart dd = d ;
			do
			{
				if (pedWayMark.isMarked(map.phi2(dd)) && pedWayMark.isMarked(map.phi2(map.phi1(dd)))
				    && !pedWayMark.isMarked(map.phi2(map.phi1(map.phi1(dd))))
				    && !pedWayMark.isMarked(map.phi2(map.phi1(map.phi1(map.phi1(dd))))))
				{
					pedWayMark.mark(d) ;
					break ;
				}

				dd = map.phi1(dd) ;

			} while (dd != d) ;
		}
	}
}

unsigned int EnvMap::mapMemoryCost()
{
	return (map.getAttributeContainer<DART>()).memorySize()
	    + (map.getAttributeContainer<VERTEX>()).memorySize()
	    + (map.getAttributeContainer<EDGE>()).memorySize()
	    + (map.getAttributeContainer<FACE>()).memorySize() ;
}

#ifndef SPATIAL_HASHING
void EnvMap::subdivideAllToMaxLevel()
{
	bool subdiv ;
	do
	{
		subdiv = false ;
		{
			CellMarker<FACE> subd(map) ;
			for (Dart d = map.begin(); d != map.end(); map.next(d))
			{
				if (!subd.isMarked(d))
				{
					subd.mark(d) ;
					if (!buildingMark.isMarked(d))
					{
						//check if subdivision is authorized
						float minDistSq = Agent::neighborDistSq_ ;
						bool subdivisable = true ;
						Dart old = map.faceOldestDart(d) ;
						unsigned int fLevel = map.faceLevel(old) ;
						map.setCurrentLevel(fLevel) ;
						PFP::VEC3 fCenter = Algo::Geometry::faceCentroid<PFP>(map, old, position) ;
						Dart fd = old ;
						do
						{
							PFP::VEC3& p = position[fd] ;
							PFP::VEC3 edge = Algo::Geometry::vectorOutOfDart<PFP>(map, fd,
							                                                      position) ;
							PFP::VEC3 proj = fCenter
							    - (p + (edge * (fCenter - p) / edge.norm2()) * edge) ;
							if (proj.norm2() < minDistSq)
							{
								subdivisable = false ;
								break ;
							}
							fd = map.phi1(fd) ;
						} while (fd != old) ;

						if (subdivisable)
						{
							map.setCurrentLevel(fLevel) ;
							Algo::IHM::subdivideFace<PFP>(map, old, position) ;
							subdiv = true ;
						}
						map.setCurrentLevel(map.getMaxLevel()) ;
					}
				}
			}
		}
	} while (subdiv) ;
}

void EnvMap::subdivideToProperLevel()
{
	bool subdiv ;
	do
	{
		subdiv = false ;
		{
			CellMarker<FACE> subd(map) ;
			for (Dart d = map.begin(); d != map.end(); map.next(d))
			{
				if (!subd.isMarked(d))
				{
					subd.mark(d) ;
					if (!refineMark.isMarked(d) && agentvect[d].size() > nbAgentsToSubdivide)
					{
						std::pair<bool, bool>& sf = subdivisableFace[d] ;
						if (sf.first == false || (sf.first == true && sf.second))
						{
							subdiv = true ;
							refineMark.mark(d) ;
							refineCandidate.push_back(d) ;
						}
					}
				}
			}
			subd.unmarkAll() ;
		}
		updateMap() ;
		refineCandidate.clear() ;
		map.setCurrentLevel(map.getMaxLevel()) ;
	} while (subdiv) ;
}

Dart EnvMap::getBelongingCell(const PFP::VEC3& pos)
{
//	assert(map.getCurrentLevel() == map.getMaxLevel()) ;
	CellMarkerStore<FACE> m(map) ;
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!m.isMarked(d))
		{
			m.mark(d) ;
			if (!map.isBoundaryFace(d) && !buildingMark.isMarked(d)
			    && Algo::Geometry::isPointInConvexFace2D<PFP>(map, d, position, pos, true)) return d ;
		}
	}

//	std::cout << "ERROR : pos not in map for getBelongingCell" << std::endl ;
	return map.begin() ;
}
#endif

#ifndef SPATIAL_HASHING
void EnvMap::foreach_neighborFace(Dart d, FunctorType& f)
{
	Dart dd = d ;
	do
	{
		Dart ddd = map.alpha1(map.alpha1(dd)) ;
		while (ddd != dd)
		{
			f(ddd) ;
			ddd = map.alpha1(ddd) ;
		}
		dd = map.phi1(dd) ;
	} while (dd != d) ;
}

// TODO Check void EnvMap::registerWallInFaces() boundary markers ?
void EnvMap::registerWallInFaces()
{
	CellMarker<FACE> m(map) ;
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!m.isMarked(d))
		{
			m.mark(d) ;

			Dart dd = d ;

			//retrieve all obstacles within its one-ring

			//first : test all edges of the face of d
			do
			{
				if (obstacleMark.isMarked(dd) && map.isBoundaryMarked(dd))
				{
					Dart dd2 = map.phi2(dd) ;
					Dart next = map.phi1(dd2) ;
					Dart previous = map.phi_1(dd2) ;
					Obstacle* o = new Obstacle(position[next], position[dd2], position[previous],
					                           position[map.phi1(next)], NULL, 0) ;
					obstvect[dd2].push_back(o) ;
				}
				dd = map.phi1(dd) ;
			} while (dd != d) ;

//				//second : test all edges of neighboring faces
//				do
//				{
//					Dart ddd = map.alpha1(map.alpha1(dd));
//					while(ddd != dd)
//					{
//						if(!buildingMark.isMarked(ddd))
//							addNeighborObstacles(obstvect[d], ddd, ddd == map.alpha_1(dd));
//						ddd = map.alpha1(ddd);
//					}
//					dd = map.phi1(dd);
//				} while(dd != d);
		}
	}

}

void EnvMap::registerObstaclesInFaces()
{
	CellMarker<FACE> m(map) ;
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!m.isMarked(d))
		{
			m.mark(d) ;
			if (!buildingMark.isMarked(d))
			{
				Dart dd = d ;

				//retrieve all obstacles within its one-ring

				//first : test all edges of the face of d
				do
				{
					if (obstacleMark.isMarked(dd))
					{
						Dart dd2 = map.phi2(dd) ;
						Dart next = map.phi1(dd2) ;
						Dart previous = map.phi_1(dd2) ;
						Obstacle* o = new Obstacle(position[dd2], position[next],
						                           position[previous], position[map.phi1(next)],
						                           NULL, 0) ;
						obstvect[d].push_back(o) ;
					}
					dd = map.phi1(dd) ;
				} while (dd != d) ;

				//second : test all edges of neighboring faces
				do
				{
					Dart ddd = map.alpha1(map.alpha1(dd)) ;
					while (ddd != dd)
					{
						// TODO Check effet de bord
						if (!buildingMark.isMarked(ddd)) addNeighborObstacles(
						    obstvect[d], ddd, ddd == map.alpha_1(dd)) ;
						ddd = map.alpha1(ddd) ;
					}
					dd = map.phi1(dd) ;
				} while (dd != d) ;
			}
		}
	}
}

void EnvMap::addNeighborObstacles(PFP::OBSTACLES& obst, Dart d, bool edgeNeighbor)
{
	Dart stop ;
	if (edgeNeighbor)
		stop = map.phi_1(map.phi_1(map.phi_1(d))) ;
	else
		stop = map.phi_1(map.phi_1(d)) ;

	Dart dd = d ;
	do
	{
		if (obstacleMark.isMarked(dd))
		{
//			if(buildingMark.isMarked(dd))
//			{
//				std::cout << "caca prout" << std::endl;
//				Dart next = map.phi1(dd);
//				Dart previous = map.phi_1(dd);
//				Obstacle* o = new Obstacle(position[dd], position[next], position[previous], position[map.phi1(next)]);
//				obst.push_back(o);
//			}
//			else
//			{
			Dart dd2 = map.phi2(dd) ;
			Dart next = map.phi1(dd2) ;
			Dart previous = map.phi_1(dd2) ;
			Obstacle* o = new Obstacle(position[dd2], position[next], position[previous],
			                           position[map.phi1(next)], NULL, 0) ;
			obst.push_back(o) ;
//			}
		}
		dd = map.phi1(dd) ;
	} while (dd != stop) ;
}

void EnvMap::agentChangeFace(Agent* agent, Dart oldFace)
{
	Dart newFace = agent->part_.d ;

	popAgentInCells(agent, oldFace) ;
	pushAgentInCells(agent, newFace) ;

	if (!refineMark.isMarked(newFace) && agentvect[newFace].size() > nbAgentsToSubdivide)
	{
		std::pair<bool, bool>& sf = subdivisableFace[newFace] ;
		if (sf.first == false || (sf.first == true && sf.second))
		{
			refineMark.mark(newFace) ;
			refineCandidate.push_back(newFace) ;
		}
	}

	if (!coarsenMark.isMarked(oldFace) && !refineMark.isMarked(oldFace)
	    && 4 * agentvect[oldFace].size() < nbAgentsToSimplify)
	{
		coarsenMark.mark(oldFace) ;
		coarsenCandidate.push_back(map.faceOldestDart(oldFace)) ;
	}
}

void EnvMap::obstacleChangeFace(Obstacle* agent, Dart newFace, Dart oldFace)
{
	popObstacleInCells(agent, oldFace) ;
	pushObstacleInCells(agent, newFace) ;
}
void EnvMap::refine()
{
	for (std::vector<Dart>::iterator it = refineCandidate.begin(); it != refineCandidate.end();
		    ++it)
		{
			Dart d = (*it) ;
			refineMark.unmark(d) ;

			//if the number of agents is big enough
			if (agentvect[d].size() > nbAgentsToSubdivide)
			{
				int fLevel = -1 ;
				Dart old = map.faceOldestDart(d) ;

				bool subdivisable = true ;

				//check if faces resulting from a subdivision are big enough
				std::pair<bool, bool>& sf = subdivisableFace[old] ;
				if (sf.first == true)
					subdivisable = sf.second ;
				else
				{
					float minSizeSq = minCellSize * minCellSize ; // diametre de vision de l'agent au carré

					fLevel = map.faceLevel(old) ;
					map.setCurrentLevel(fLevel) ;
					PFP::VEC3 fCenter = Algo::Geometry::faceCentroid<PFP>(map, old, position) ;
					Dart fd = old ;
					do
					{
						PFP::VEC3& p = position[fd] ;
						PFP::VEC3 edge = Algo::Geometry::vectorOutOfDart<PFP>(map, fd, position) ;
						PFP::VEC3 proj = fCenter - (p + (edge * (fCenter - p) / edge.norm2()) * edge) ;
						if (proj.norm2() < minSizeSq)
						{
							subdivisable = false ;
							break ;
						}
						fd = map.phi1(fd) ;
					} while (fd != old) ;
					map.setCurrentLevel(map.getMaxLevel()) ;

					sf.first = true ;
					sf.second = subdivisable ;
				}

				if (subdivisable)
				{
					if (fLevel == -1) fLevel = map.faceLevel(old) ;

					sf.first = false ;

					PFP::AGENTS oldAgents(agentvect[old]) ;
					PFP::OBSTACLEVECT oldObst(obstvect[old]) ;
					PFP::OBSTACLEVECT oldNeighborObst(neighborObstvect[old]) ;
					for (PFP::AGENTS::iterator ait = oldAgents.begin(); ait != oldAgents.end(); ++ait)
						popAgentInCells(*ait, old) ;
					for (PFP::OBSTACLEVECT::iterator ait = oldObst.begin(); ait != oldObst.end(); ++ait)
						register_pop(*ait, (*ait)->index) ;
					for (PFP::OBSTACLEVECT::iterator ait = oldNeighborObst.begin();ait != oldNeighborObst.end(); ++ait)
						register_pop(*ait, (*ait)->index) ;
					neighborAgentvect[old].clear() ;

					map.setCurrentLevel(fLevel) ;
					Algo::IHM::subdivideFace<PFP>(map, old, position) ;
					CellMarkerStore<FACE> newF(map) ;
					unsigned int degree = 0 ;
					Dart dd = old ;
					do
					{
						++degree ;
						newF.mark(dd) ;
						dd = map.phi1(dd) ;
					} while (dd != old) ;

					if (degree == 3)
					{
						Dart centerFace = map.phi2(map.phi1(old)) ;
						newF.mark(centerFace) ;
					}
					map.setCurrentLevel(map.getMaxLevel()) ;

					//agents contained in the subdivided cell are pushed correctly
					for (PFP::AGENTS::iterator ait = oldAgents.begin(); ait != oldAgents.end(); ++ait)
					{
						resetAgentInFace(*ait) ;
						pushAgentInCells(*ait, (*ait)->part_.d) ;
					}

					//same for obstacles contained
					for (PFP::OBSTACLEVECT::iterator ait = oldObst.begin(); ait != oldObst.end(); ++ait)
					{
						resetPartSubdiv(*ait);
						update_registration(*ait) ;
					}

					//same for adjacent obstacles
					for (PFP::OBSTACLEVECT::iterator ait = oldNeighborObst.begin();
					    ait != oldNeighborObst.end(); ++ait)
					{

						update_registration(*ait) ;
					}

					//retrieve neighbors agents from onering cells
					dd = old ;
					do
					{
						Dart d3 = dd ;
						do
						{
							Dart d4 = map.alpha1(map.alpha1(d3)) ;
							PFP::AGENTS& nad3 = neighborAgentvect[d3] ;
							while (d4 != d3)
							{
								if (!newF.isMarked(d4))
								{
									PFP::AGENTS& ad4 = agentvect[d4] ;
									nad3.insert(nad3.end(), ad4.begin(), ad4.end()) ;
								}
								d4 = map.alpha1(d4) ;
							}
							d3 = map.phi1(d3) ;
						} while (d3 != dd) ;

						map.setCurrentLevel(fLevel) ;
						dd = map.phi1(dd) ;
						map.setCurrentLevel(map.getMaxLevel()) ;
					} while (dd != old) ;

					if (degree == 3)
					{
						Dart centerFace = map.phi2(map.phi1(old)) ;
						Dart d3 = centerFace ;
						do
						{
							Dart d4 = map.alpha1(map.alpha1(d3)) ;
							PFP::AGENTS& nad3 = neighborAgentvect[d3] ;
							while (d4 != d3)
							{
								if (!newF.isMarked(d4))
								{
									PFP::AGENTS& ad4 = agentvect[d4] ;
									nad3.insert(neighborAgentvect[d3].end(), ad4.begin(), ad4.end()) ;
								}
								d4 = map.alpha1(d4) ;
							}
							d3 = map.phi1(d3) ;
						} while (d3 != centerFace) ;
					}
				}
			}
		}
		refineCandidate.clear() ;

}
void EnvMap::coarse()
{
	// On recrée une liste des faces à simplifier en empêchant les doublons
		// On en profite pour vérifier les conditions de simplifications
		std::vector<Dart> checkCoarsenCandidate ;
		checkCoarsenCandidate.reserve(coarsenCandidate.size()) ;

		for (unsigned int it = 0; it < coarsenCandidate.size(); ++it)
		{
			Dart old = coarsenCandidate[it] ;
			bool oldIsMarked = coarsenMark.isMarked(old) ;
			coarsenMark.unmark(old) ;

			unsigned int fLevel = map.faceLevel(old) ;

			if (oldIsMarked && fLevel > 0 && map.getDartLevel(old) < fLevel)
			{
				unsigned int cur = map.getCurrentLevel() ;
				map.setCurrentLevel(fLevel - 1) ;

				if (map.faceIsSubdividedOnce(old))
				{
					// on compte le nombre d'agents dans les sous-faces
					// on en profite pour compter le degré de la face grossière
					unsigned int degree = 0 ;
					unsigned int nbAgents = 0 ;
					Dart fit = old ;
					do
					{
						nbAgents += agentvect[fit].size() ;
						++degree ;
						coarsenMark.unmark(fit) ;
	// TODO Optimisation déjà faite
	//					unsigned int start = it + 1;
	//					unsigned int fEmb = map.getEmbedding<FACE>(fit) ;
	//					while(start < coarsenCandidate.size())
	//					{
	//						if(map.getEmbedding<FACE>(coarsenCandidate[start]) == fEmb)
	//						{
	//							coarsenCandidate[start] = coarsenCandidate.back() ;
	//							coarsenCandidate.pop_back() ;
	//						}
	//						else
	//							++start ;
	//					}

						fit = map.phi1(fit) ;
					} while (fit != old) ;


					//Loop subdivision
					if (degree == 3)
					{
						map.setCurrentLevel(fLevel) ;
						Dart centerFace = map.phi2(map.phi1(old)) ;
						nbAgents += agentvect[centerFace].size() ;
						coarsenMark.unmark(centerFace) ;
	// TODO Optimisation déjà faite
	//					unsigned int start = it + 1;
	//					unsigned int fEmb = map.getEmbedding<FACE>(centerFace) ;
	//					while(start < coarsenCandidate.size())
	//					{
	//						if(map.getEmbedding<FACE>(coarsenCandidate[start]) == fEmb)
	//						{
	//							coarsenCandidate[start] = coarsenCandidate.back() ;
	//							coarsenCandidate.pop_back() ;
	//						}
	//						else
	//							++start ;
	//					}
						map.setCurrentLevel(fLevel - 1) ;
					}
					if (nbAgents < nbAgentsToSimplify) checkCoarsenCandidate.push_back(old) ;
				}
				map.setCurrentLevel(cur) ;
			}
		}
		coarsenCandidate.clear() ;

//		 On réalise la simplification (les conditions ont déjà été vérifiées)
		for (unsigned int it = 0; it < checkCoarsenCandidate.size(); ++it)
		{
			Dart old = checkCoarsenCandidate[it] ;

			unsigned int fLevel = map.faceLevel(old) ;
			unsigned int cur = map.getCurrentLevel() ;
			map.setCurrentLevel(fLevel - 1) ;

			// on compte le degré de la face grossière
			unsigned int degree = 0 ;
			Dart fit = old ;
			do
			{
				++degree ;
				fit = map.phi1(fit) ;
			} while (fit != old) ;

			PFP::AGENTS agents ;
			PFP::OBSTACLEVECT obst ;
			PFP::OBSTACLEVECT neighborObst ;
			//premier tour pour les présents
			fit = old ;
			do
			{
				PFP::AGENTS a(agentvect[fit]) ;
				PFP::OBSTACLEVECT ob(obstvect[fit]) ;


				agents.insert(agents.end(), a.begin(), a.end()) ;


				map.setCurrentLevel(map.getMaxLevel()) ;
				for(PFP::OBSTACLEVECT::iterator ait = ob.begin(); ait != ob.end(); ++ait)
				{
					register_pop(*ait, (*ait)->index) ;
					obst.push_back(*ait);
				}



				for (PFP::AGENTS::iterator ait = a.begin(); ait != a.end(); ++ait)
					popAgentInCells(*ait, fit) ;
				map.setCurrentLevel(fLevel - 1) ;
				Dart nf = map.phi2(fit) ;
				if (!map.faceIsSubdivided(nf))
				{
					map.setCurrentLevel(fLevel) ;
					PFP::AGENTS& an = agentvect[nf] ;
					PFP::OBSTACLEVECT resetob = obstvect[nf] ;
					for (PFP::AGENTS::iterator ait = an.begin(); ait != an.end(); ++ait)
					{
						if ((*ait)->part_.d == map.phi1(nf)) (*ait)->part_.d = nf ;
					}
					for (PFP::OBSTACLEVECT::iterator ait = resetob.begin(); ait != resetob.end(); ++ait)
					{
						MovingObstacle* mo = (*ait)->mo ;
						if (mo != NULL)
						{
							resetPart(mo,nf) ;
						}
					}
					map.setCurrentLevel(fLevel - 1) ;
				}
				fit = map.phi1(fit) ;
			} while (fit != old) ;

			if (degree == 3)
			{
				map.setCurrentLevel(fLevel) ;
				Dart centerFace = map.phi2(map.phi1(old)) ;
				PFP::AGENTS a(agentvect[centerFace]) ;
				PFP::OBSTACLEVECT ob(obstvect[centerFace]) ;

				agents.insert(agents.end(), a.begin(), a.end()) ;
				map.setCurrentLevel(map.getMaxLevel()) ;
				for(PFP::OBSTACLEVECT::iterator ait = ob.begin(); ait != ob.end(); ++ait)
				{
					register_pop(*ait, (*ait)->index) ;
					obst.push_back(*ait);
				}
				for (PFP::AGENTS::iterator ait = a.begin(); ait != a.end(); ++ait)
					popAgentInCells(*ait, centerFace) ;
				map.setCurrentLevel(fLevel - 1) ;
			}

			//deuxieme tour concernant les voisins
			fit = old ;
			do
			{

				PFP::OBSTACLEVECT nob(neighborObstvect[fit]) ;

				map.setCurrentLevel(map.getMaxLevel()) ;
				for(PFP::OBSTACLEVECT::iterator ait = nob.begin(); ait != nob.end(); ++ait)
				{
					register_pop(*ait, (*ait)->index) ;
					neighborObst.push_back(*ait);
				}
				map.setCurrentLevel(fLevel - 1) ;

				fit = map.phi1(fit) ;
			} while (fit != old) ;

			if (degree == 3)
			{
				map.setCurrentLevel(fLevel) ;
				Dart centerFace = map.phi2(map.phi1(old)) ;
				PFP::OBSTACLEVECT nob(neighborObstvect[centerFace]) ;
				map.setCurrentLevel(map.getMaxLevel()) ;
				for(PFP::OBSTACLEVECT::iterator ait = nob.begin(); ait != nob.end(); ++ait)
				{
					register_pop(*ait, (*ait)->index) ;
					neighborObst.push_back(*ait);
				}
				map.setCurrentLevel(fLevel - 1) ;
			}

			neighborAgentvect[old].clear() ;

			// TODO Check with optimisation

	//
	//		CGoGNout<<"Avant pop"<<CGoGNendl;
	//		CGoGNout<<"Obst dans la case : ";
	//		for (PFP::OBSTACLEVECT::iterator ait = obst.begin(); ait != obst.end(); ++ait)
	//			CGoGNout<<(*ait)->index<<" ; ";
	//		CGoGNout<<CGoGNendl;
	//		CGoGNout<<"Obst voisins : ";
	//		for (PFP::OBSTACLEVECT::iterator ait = neighborObst.begin(); ait != neighborObst.end(); ++ait)
	//			CGoGNout<<(*ait)->index<<" ; ";
	//		CGoGNout<<CGoGNendl;


			map.setCurrentLevel(fLevel - 1) ;

			Algo::IHM::coarsenFace<PFP>(map, old, position) ;

			std::pair<bool, bool>& sf = subdivisableFace[old] ;
			sf.first = true ;
			sf.second = true ;

			map.setCurrentLevel(map.getMaxLevel()) ;

			for (PFP::AGENTS::iterator itA = agents.begin(); itA != agents.end(); ++itA)
			{
				(*itA)->part_.d = old ;
				pushAgentInCells(*itA, old) ;
			}
			for (PFP::OBSTACLEVECT::iterator ait = obst.begin(); ait != obst.end(); ++ait)
			{
				resetObstPartInFace(*ait, old) ;
				update_registration(*ait) ;
			}
			for (PFP::OBSTACLEVECT::iterator ait = neighborObst.begin(); ait != neighborObst.end(); ++ait)
			{
				update_registration(*ait) ;
			}

			Dart dd = old ;
			do
			{
				Dart ddd = map.alpha1(map.alpha1(dd)) ;
				while (ddd != dd)
				{
					neighborAgentvect[old].insert(neighborAgentvect[old].end(), agentvect[ddd].begin(), agentvect[ddd].end()) ;
					ddd = map.alpha1(ddd) ;
				}
				dd = map.phi1(dd) ;
			} while (dd != old) ;

			if (fLevel > 1 && !coarsenMark.isMarked(old) && agentvect[old].size() < nbAgentsToSimplify)
			{
				coarsenMark.mark(old) ;
				coarsenCandidate.push_back(map.faceOldestDart(old)) ;
			}

			map.setCurrentLevel(cur) ;
		}
		map.setCurrentLevel(map.getMaxLevel()) ;
		if (coarsenCandidate.size() > 0) updateMap() ;
}
void EnvMap::updateMap()
{
	assert(map.getCurrentLevel() == map.getMaxLevel()) ;

	refine();
	coarse();


}

void EnvMap::resetAgentInFace(Agent* agent)
{
	VEC3 pos = agent->part_.getPosition() ;
	agent->part_.move(Algo::Geometry::faceCentroid<PFP>(map, agent->part_.d, position)) ;
	agent->part_.setState(FACE) ;
	agent->part_.move(pos) ;
}
#endif

#ifdef SPATIAL_HASHING
Geom::Vec2ui EnvMap::agentPositionCell(Agent* a)
{
	VEC3 relativePos = a->pos - geometry.min() ;
	relativePos /= minCellSize ;
	return Geom::Vec2ui(relativePos[0], relativePos[1]) ;
}

const std::vector<Agent*>& EnvMap::getNeighbors(Agent* a)
{
	Geom::Vec2ui c = agentPositionCell(a) ;
	return ht_agents[c] ;
}

const std::vector<Agent*>& EnvMap::getNeighborAgents(Agent* a)
{
	Geom::Vec2ui c = agentPositionCell(a) ;
	return ht_neighbor_agents[c] ;
}

void EnvMap::getOneRingNeighbors(Agent* a, std::vector<Agent*>& neighbors)
{
	Geom::Vec2ui c = agentPositionCell(a) ;
	for(int ii = -1 ; ii <= 1 ; ++ii)
	{
		for(int jj = -1 ; jj <= 1 ; ++jj)
		{
			if (ii != 0 || jj != 0)
			{
				Geom::Vec2ui cc = c + Geom::Vec2ui(ii, jj) ;
				const std::vector<Agent*>& v = ht_agents[cc] ;
				neighbors.insert(neighbors.end(), v.begin(), v.end()) ;
			}
		}
	}
}

void EnvMap::addAgentInGrid(Agent* a)
{
	Geom::Vec2ui c = agentPositionCell(a) ;
	addAgentInGrid(a,c) ;
}

void EnvMap::addAgentInGrid(Agent* a, Geom::Vec2ui c)
{
	ht_agents[c].push_back(a) ;

	for(int ii = -1; ii <= 1; ++ii)
	{
		for(int jj = -1; jj <= 1; ++jj)
		{
			if(!(ii == 0 && jj == 0))
			{
				Geom::Vec2ui cc = c + Geom::Vec2ui(ii, jj) ;
				ht_neighbor_agents[cc].push_back(a) ;
			}
		}
	}
}

void EnvMap::removeAgentFromGrid(Agent* a)
{
	Geom::Vec2ui c = agentPositionCell(a) ;
	removeAgentFromGrid(a,c) ;
}

void EnvMap::removeAgentFromGrid(Agent* a, Geom::Vec2ui c)
{
	removeElementFromVector<Agent*>(ht_agents[c], a) ;
	if (ht_agents[c].empty()) ht_agents.erase(c) ;

	for(int ii = -1; ii <= 1; ++ii)
	{
		for(int jj = -1; jj <= 1; ++jj)
		{
			if(!(ii == 0 && jj == 0))
			{
				Geom::Vec2ui cc = c + Geom::Vec2ui(ii, jj) ;
				removeElementFromVector<Agent*>(ht_neighbor_agents[cc], a) ;
				if (ht_neighbor_agents[c].empty()) ht_neighbor_agents.erase(c) ;
			}
		}
	}
}

#endif