env_map.cpp

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

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

#include "Topology/generic/mapBrowser.h"

#include "env_generator.h"
#include <dirent.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") ;

	positionScenary = mapScenary.addAttribute<VEC3, VERTEX>("position") ;
	normalScenary = mapScenary.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) ;
	
	this->config = config;

	geometry.reset() ;
	geometry.addPoint(bottomLeft) ;
	geometry.addPoint(topRight) ;
	minCellSize = minSize ;
	maxCellSize = maxSize ;
	std::cout << " - Geometry : " << geometry ;
	std::cout << " - Geometry size : " << geometry.size(0)<<" x "<<geometry.size(1) ;
	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) ;
			CityGenerator::generateGrid<PFP>(*this) ;
//			CityGenerator::generateTrianGrid<PFP>(*this,obstacleMark, buildingMark);
//			CityGenerator::generateCity<PFP>(*this,0,500.0f) ;
			break ;
		case 1 :
//			CityGenerator::generateGrid<PFP>(*this) ;
			CityGenerator::generateTrianGrid<PFP>(*this,obstacleMark, buildingMark);
			break ;
		case 2 :
//			CityGenerator::generateGrid<PFP>(*this) ;
			CityGenerator::generateTrianGrid<PFP>(*this,obstacleMark, buildingMark);
			// CityGenerator::generateMall<PFP>(map, position, obstacleMark, buildingMark, sideSize);
			break ;
		case 3 :
//			CityGenerator::generateGrid<PFP>(*this) ;
			CityGenerator::generateTrianGrid<PFP>(*this,obstacleMark, buildingMark);
//			CityGenerator::generateCity<PFP>(*this,10,500.0f) ;
			break ;
		case 4 :
			CityGenerator::generateTrianGrid<PFP>(*this,obstacleMark, buildingMark);

//			CityGenerator::generateGrid<PFP>(*this) ;
			break ;
		case 5 :
		{
			std::cout << "?" << std::endl;
//			std::string filename = "./svg/planet.svg" ;
			std::string filename = "./svg/mapRoads.svg" ;
//			std::string filename = "./svg/simpleCross.svg" ;
			Algo::Surface::Import::importSVG<PFP>(map, filename, position, obstacleMark, buildingMark) ;

			std::string filename2 = "./svg/mapBuild.svg" ;
			Algo::Surface::Import::importSVG<PFP>(mapScenary, filename2, positionScenary, obstacleMarkS, buildingMarkS) ;

			Geom::BoundingBox<PFP::VEC3> bb2 ;
			bb = Algo::Geometry::computeBoundingBox<PFP>(map, position) ;
			bb2 = Algo::Geometry::computeBoundingBox<PFP>(mapScenary, positionScenary) ;
			bb.addPoint(bb2.min());
			bb.addPoint(bb2.max());

			std::cout << "bb " << bb.min() << " & " << bb.max() << std::endl;

//			CityGenerator::planetify<PFP>(map, position, 100.0f, bb);
//			CityGenerator::planetify<PFP>(mapScenary, positionScenary, 100.0f, bb);

			std::vector<Dart> v;
			TraversorF<PFP::MAP> tF(mapScenary);
			for(Dart d = tF.begin() ; d != tF.end() ; d = tF.next())
			{
				v.push_back(d);
			}

			std::cout << "builds " << v.size() << std::endl;

			for(std::vector<Dart>::iterator it = v.begin() ; it != v.end() ; it++)
			{
				Dart d = *it;

				//test if convex
				bool convex=true;

				Dart dd = mapScenary.phi1(d);
				Dart dN = mapScenary.phi1(dd);
				Dart dNN = mapScenary.phi1(dN);

				VEC3 p1, p2, p3, v1, v2;
				p1 = positionScenary[dd];
				p2 = positionScenary[dN];
				p3 = positionScenary[dNN];

				v1 = VEC3(p2-p1);
				v2 = VEC3(p3-p2);

				float sign = (v1^v2)[2];

				while(convex && dd!=d)
				{
					dd=dN;
					dN = dNN;
					dNN = mapScenary.phi1(dNN);

					p1 = positionScenary[dd];
					p2 = positionScenary[dN];
					p3 = positionScenary[dNN];

					v1 = VEC3(p2-p1);
					v2 = VEC3(p3-p2);

					float sign2 = (v1^v2)[2];
					if((sign<0 && sign2>0) || (sign>0 && sign2<0))
							convex=false;
				}

				//make extrusion/roof/..
				if(convex)
				{
					if(Algo::Surface::Geometry::convexFaceArea<PFP>(mapScenary, d, positionScenary)<20.0f) //if big enough create building with "stairs"
						CityGenerator::extrudeBuilding<PFP>(mapScenary,positionScenary, d,1+rand()%2,10.0f*(1.0f+rand()/RAND_MAX));
					else //create building with simple roof
						CityGenerator::extrudeBuilding<PFP>(mapScenary,positionScenary, d,3,10.0f*(1.0f+rand()/RAND_MAX));
				}
				else //building with no roof
				{
					CityGenerator::extrudeBuilding<PFP>(mapScenary,positionScenary, d,0,5.0f+5.0f*(1.0f+2.0f*rand()/RAND_MAX));
				}
			}

			//triangulation for rendering
			Algo::Surface::Modelisation::EarTriangulation<PFP> et(mapScenary);
			et.triangule();

//			Algo::Modelisation::EarTriangulation<PFP> et2(map);
//			et2.triangule();

			//subdivision to create footpath
			MapBrowserSelector mbs(map, SelectorDartNoBoundary<PFP::MAP>(map));
			map.setBrowser(&mbs);
			Algo::Surface::Modelisation::TwoNPlusOneSubdivision<PFP,PFP::TVEC3, VEC3>(map,position,1.0f/5.0f);
			map.setBrowser(NULL);
			markPedWay();

			scale(20.0f);
		}
		break;
		case 6:
			CityGenerator::generatePlanet<PFP>(*this);
			CityGenerator::generateCity<PFP>(*this,30, 1000.0f, 20.0f) ;
			break;
		case 7 :

//			CityGenerator::importObj<PFP>(*this, "./meshRessources/simpleSlugSmoothTest.obj");

//			CityGenerator::importObj<PFP>(*this, "./meshRessources/knot3Centered.obj");
			CityGenerator::importObj<PFP>(*this, "./meshRessources/testTaches.obj");
			scale(150.0f);
			CityGenerator::generateCity<PFP>(*this,300, 150.0f, 15.0f) ;
			break;
		case 8 :

//			CityGenerator::importObj<PFP>(*this, "./meshRessources/simpleSlugSmoothTest.obj");

			CityGenerator::importObj<PFP>(*this, "./meshRessources/knot3CenteredSubSmooth.obj");

			scale(150.0f);
//			CityGenerator::generateCity<PFP>(*this,1000, 150.0f,5.0f) ;
			break;
		case 9 :
			CityGenerator::importObj<PFP>(*this, "./meshRessources/simpleSlugSmoothTest.obj");
// 			CityGenerator::importObj<PFP>(*this, "./meshRessources/LimSol.obj");
			scale(150.0f);
// 			CityGenerator::generateCity<PFP>(*this,200, 150.0f,15.0f) ;
			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

	if(!normal.isValid())
		normal = map.addAttribute<PFP::VEC3, VERTEX>("normal") ;

	Algo::Surface::Geometry::computeNormalVertices<PFP>(map, position, normal) ;

	if(config==5 && !normalScenary.isValid())
	{
		normalScenary = mapScenary.addAttribute<PFP::VEC3, VERTEX>("normal") ;

		Algo::Surface::Geometry::computeNormalVertices<PFP>(mapScenary, positionScenary, normalScenary) ;
	}
}

void EnvMap::initGL()
{
#ifdef EXPORTING3
	std::vector<std::string> filenames;
	std::vector<std::string> texturenames;
	
	std::ostringstream tmp;
	tmp << "./meshRessources/scenario"  << config << "/" ;
			
	std::string dir = tmp.str();
	
	DIR *dp;
	struct dirent *dirp;
	if((dp  = opendir(dir.c_str())) == NULL)
	{
		cout << "Error(" << errno << ") opening " << dir << endl;
		return;
	}

	while ((dirp = readdir(dp)) != NULL)
	{
		if(string(dirp->d_name).find(".obj")!=string::npos)
		{
			filenames.push_back(dir+string(dirp->d_name));
			std::cout << dir+string(dirp->d_name) << std::endl;
		}

		if(string(dirp->d_name).find(".png")!=string::npos)
		{
			texturenames.push_back(dir+string(dirp->d_name));

			std::cout << dir+string(dirp->d_name) << std::endl;
		}
	}

	std::sort(filenames.begin(), filenames.end());
	std::sort(texturenames.begin(), texturenames.end());

	std::reverse(filenames.begin(), filenames.end());
	std::reverse(texturenames.begin(), texturenames.end());

//	filenames.push_back(std::string("./meshRessources/cityTex/Building11.obj"));
//	texturenames.push_back(std::string("./meshRessources/cityTex/AO_Buildings11.png"));


	Geom::BoundingBox<PFP::VEC3> bbTest ;
	std::vector<VertexAttribute<VEC3> > position_nmap;

	for(unsigned int i = 0 ; i < filenames.size() ; ++i)
	{
		std::cout << "file " << filenames[i] << std::endl;

		PFP::MAP * nmap = new PFP::MAP();

		std::vector<std::string> attrNames ;
		Algo::Surface::Import::OBJModel<PFP2> * obj = new Algo::Surface::Import::OBJModel<PFP2>(*nmap);
		obj->import(filenames[i],attrNames);

		VertexAttribute<VEC3> position_Export = nmap->getAttribute<VEC3, VERTEX>(attrNames[0]) ;

		position_nmap.push_back(position_Export);

//		TraversorV<PFP2::MAP> tV(*nmap);

//		for(Dart d = tV.begin() ; d != tV.end() ; d = tV.next())
////		{
////			position_Export[d] *= 100.0f;
////			position_Export[d] += VEC3(2000,0,0);
////		}



		Geom::BoundingBox<PFP::VEC3> bbTest2 = Algo::Geometry::computeBoundingBox<PFP>(*nmap, position_Export) ;
		bbTest.addPoint(bbTest2.min());
		bbTest.addPoint(bbTest2.max());

		Utils::VBO * texcoordVBO = new Utils::VBO();
		Utils::VBO * positionVBO = new Utils::VBO();
		Utils::VBO * normalVBO = new Utils::VBO();

		Utils::Texture<2,Geom::Vec3uc> * texture = new Utils::Texture<2,Geom::Vec3uc>(GL_UNSIGNED_BYTE);

		if (texture->load(texturenames[i]))
				texture->update();
		else
			std::cout << "problem : loading texture" << std::endl;

		texture->setWrapping(GL_CLAMP_TO_EDGE);

		ShaderCustomTex * shaderTex = new ShaderCustomTex();
		shaderTex->setAttributePosition(positionVBO);
		shaderTex->setAttributeTexCoord(texcoordVBO);
		shaderTex->setTextureUnit(GL_TEXTURE0);
		shaderTex->setTexture(texture);

		if(filenames[i].find("ground.obj")!=string::npos)
			shaderTex->setBaseColor(Geom::Vec4f(0.6,0.8,0.6,0));
		else if(filenames[i].find("road.obj")!=string::npos)
			shaderTex->setBaseColor(Geom::Vec4f(0.6,0.6,0.6,0));
		else if(filenames[i].find("pedway.obj")!=string::npos)
			shaderTex->setBaseColor(Geom::Vec4f(0.9,0.9,0.9,0));

		glEnable(GL_TEXTURE_2D);

		nmap->setBrowser(NULL);

		if (!obj->hasNormals())
		{
			VertexAttribute<VEC3> normal = nmap->getAttribute<VEC3, VERTEX>("normal") ;
			if(!normal.isValid())
				normal = nmap->addAttribute<VEC3, VERTEX>("normal") ;

			Algo::Surface::Geometry::computeNormalVertices<PFP2>(*nmap, obj->m_positions, normal) ;
			obj->setNormalAttribute(normal);
		}

		unsigned int nbIndices = obj->createSimpleVBO_PTN(positionVBO,texcoordVBO,normalVBO);

		m_map_Export.push_back(nmap);
		m_obj_Export.push_back(obj);

		m_texture_Export.push_back(texture);
		m_texcoordVBO_Export.push_back(texcoordVBO);
		m_positionVBO_Export.push_back(positionVBO);
		m_normalVBO_Export.push_back(normalVBO);
		m_shaderTex_Export.push_back(shaderTex);

		m_nbIndice_Export.push_back(nbIndices);
	}

	std::cout << "bb " << bbTest.min() << " & " << bbTest.max() << std::endl;
#endif
}

void EnvMap::draw()
{
#ifdef EXPORTING3
	glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);

	glEnable(GL_POLYGON_OFFSET_FILL) ;

	for(unsigned int i = 0 ; i < m_obj_Export.size() ; ++i)
	{
		glPolygonOffset( std::max(0.2f*i,-0.75f),std::max(0.2f*i,-0.75f)) ;

		unsigned int nbIndices = m_obj_Export[i]->createSimpleVBO_PTN( m_positionVBO_Export[i], m_texcoordVBO_Export[i], m_normalVBO_Export[i]);

		m_shaderTex_Export[i]->activeTexture();
		m_shaderTex_Export[i]->enableVertexAttribs();
//		glDrawArrays(GL_TRIANGLES, 0, m_nbIndice_Export[i]);
		glDrawArrays(GL_TRIANGLES, 0, nbIndices);
		m_shaderTex_Export[i]->disableVertexAttribs();
	}

	glDisable(GL_POLYGON_OFFSET_FILL) ;

//glDisable(GL_LIGHTING);
//	glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
//
//	unsigned int nbIndices = m_obj_Export->createSimpleVBO_PTN( m_positionVBO_Export, m_texcoordVBO_Export, m_normalVBO_Export);
//
//	m_shaderTex_Export->activeTexture();
//	m_shaderTex_Export->enableVertexAttribs();
//	glDrawArrays(GL_TRIANGLES, 0, m_nbIndice_Export);
//	m_shaderTex_Export->disableVertexAttribs();
//
//glEnable(GL_LIGHTING);
#endif
}

void EnvMap::scale(float val)
{
	TraversorV<PFP::MAP> tV(map);
	for(Dart d = tV.begin() ; d != tV.end() ; d = tV.next())
		position[d] *= val;

	TraversorV<PFP::MAP> tV2(mapScenary);
	for(Dart d = tV2.begin() ; d != tV2.end() ; d = tV2.next())
		positionScenary[d] *= val;

}

bool EnvMap::is_insideConvexCell2D(VEC3 p, Dart d)
{
//	return false;
	VEC3 vec, norm, p1,p2;
	Dart dd = d,ddd;

	do
	{
		ddd = map.phi1(dd) ;
		 p1 = position[dd];
		 p2 = position[ddd];

		vec = VEC3(p2 - p1);
		norm[0] = vec[1];
		norm[1] = -vec[0];

		vec = VEC3(p2 -p);
		if (vec*norm < 0){
//			CGoGNout<<"not inside !"<<CGoGNendl;
			return false;

		}

		dd = ddd ;
	} while (dd != d) ;
//	CGoGNout<<"c'est dedans !"<<CGoGNendl;
	return true;
}

void EnvMap::markPedWay()
{
	//split all pedway with 3 roads intersection (to avoid spikes)
	CellMarker<FACE> treat(map) ;
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!treat.isMarked(d))
		{
			treat.mark(d) ;
			//intern face for intersection
			if(map.faceDegree(d)==3)
			{
				Dart dd = d;
				do
				{
					//get "corner" quadrangular faces around and split them
					Dart dSplit = map.alpha1(map.alpha1(dd));
					map.splitFace(map.phi1(dSplit), map.phi_1(dSplit));
					treat.mark(dSplit);
					treat.mark(map.phi2(map.phi1(dSplit)));
					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) ;
			//check around the face
			Dart dd = d ;
			do
			{
				//if any vertex
				Dart ddd = dd ;
				do
				{
					//is an obstacle
					if (obstacleMark.isMarked(ddd))
					{
						//then mark as pedway
						pedWayMark.mark(d) ;
						break;
					}
					ddd = map.alpha1(ddd) ;
				} while(ddd != dd);

				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::Surface::Geometry::faceCentroid<PFP>(map, old, position) ;
						Dart fd = old ;
						do
						{
							PFP::VEC3& p = position[fd] ;
							PFP::VEC3 edge = Algo::Surface::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::Surface::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 (!buildingMark.isMarked(d) && Algo::Surface::Geometry::isPointInConvexFace2D<PFP>(map, d, position, pos, true))
				return d ;
		}
	}

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

Dart EnvMap::getBelongingCellOnSurface(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 (!buildingMark.isMarked(d) && Algo::Surface::Geometry::isPointInConvexFace<PFP>(map, d, position, pos, true))
			if (!buildingMark.isMarked(d) && isInsideFace3D(pos,d) )
			{
					return d ;
			}
		}
	}

	std::cout << "ERROR : pos not in map for getBelongingCellonSurface " << pos << 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.isBoundaryMarked2(dd))
				{
					Dart dd2 = map.phi2(dd) ;
					Dart next = map.phi1(dd2) ;

					Obstacle* o = new Obstacle(position[next], position[dd2], 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) ;

						Obstacle* o = new Obstacle(position[dd2], position[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) ;

			Obstacle* o = new Obstacle(position[dd2], position[next],  NULL, 0) ;
			obst.push_back(o) ;
//			}
		}
		dd = map.phi1(dd) ;
	} while (dd != stop) ;
}

void EnvMap::pushObstacleInCells(Obstacle * o)// réenregistre l'obstacle en question
{
	MovingObstacle * mo = o->mo;
	if (mo != NULL)
	{
		int n = o->index;

		VEC3 p1 = o->p1;
		VEC3 p2 =  o->p2;

		Dart d1=NIL;
		Dart d2=NIL;
		std::vector<Dart> memo;
		d1=mo->parts_[n]->d;

		memo = mo->getMemoCross(p1,p2,d1,d2);

		if(map.sameFace(d1,d2))
		{
			pushObstacleInOneRingCells(o , d1 , n);
		}
		else
		{

			pushObstacleInCells(o, n, memo);
		}
	}
}

Dart EnvMap::popAndPushObstacleInCells(Obstacle* o, int n)// maj de l'enregistrement
{
	MovingObstacle * mo = o->mo;
	VEC3 p1 = o->p1;
	VEC3 p2 = o->p2;

	Dart d1=NIL;
	Dart d2=NIL;
	std::vector<Dart> memo;
	d1=mo->parts_[n]->d;
//	bool modif=false;

//	if(p1->crossCell != CGoGN::Algo::MovingObjects::NO_CROSS
//		|| p2->crossCell != CGoGN::Algo::MovingObjects::NO_CROSS)
//	{
		memo = mo->getMemoCross(p1,p2,d1,d2);
//		memo.sort();
//		modif=true;
//	}
//	else if(!map.sameFace(p1->d,p2->d))
//	{
//		memo = getMemoCross(p1,p2->getPosition());
//		memo.sort();
//	 	if(belonging_cells[n] != memo)
//			modif=true;
//	}
//	if (mo->index==12 && o->index ==1) CGoGNout<<"popAndPush :"<<CGoGNendl;
//	if(modif)
	{
		popObstacleInCells(o, n);

		if(map.sameFace(d1,d2))
		{
			pushObstacleInOneRingCells(o , d1 , n);
		}
		else
		{
			pushObstacleInCells(o, n, memo);
		}
	}

	return d1;
}

void EnvMap::pushObstacleInOneRingCells(Obstacle * o, Dart d, int n)
{
	MovingObstacle * mo = o->mo;

	assert(map.getCurrentLevel() == map.getMaxLevel());

	addElementToVector<Obstacle*>(obstvect[d],o);

//	mo->belonging_cells[n].clear();
	mo->belonging_cells[n].push_back(d);
	mo->addGeneralCell (d);
//	mo->neighbor_cells[n].clear();
//	if(o->index==1 && mo->index==12)	 CGoGNout <<"push : "<< d << CGoGNendl;
	Dart dd = d;
	do
	{
		Dart ddd = map.alpha1(map.alpha1(dd));
		while(ddd != dd)
		{
			if (!map.isBoundaryMarked2(ddd))
			{
				pushObstNeighborInCells(o, ddd);
			}

			mo->neighbor_cells[n].push_back(ddd);
			ddd = map.alpha1(ddd);
		}
		dd = map.phi1(dd);
	} while(dd != d);
}

void EnvMap::pushObstacleInCells(Obstacle* o, int n, const std::vector<Dart>& memo_cross)
{
	if(memo_cross.empty())
	{
		displayMO(o);
	}
	assert(!memo_cross.empty());
	MovingObstacle * mo = o->mo;

//	mo->belonging_cells[n].clear();
//	mo->neighbor_cells[n].clear();
	mo->belonging_cells[n] = memo_cross;
//	if (mo->index==12 && o->index ==1) CGoGNout<<"push : "<<CGoGNendl;
	for (std::vector<Dart>::iterator it = mo->belonging_cells[n].begin();	it != mo->belonging_cells[n].end(); ++it)
	{
//			if(o->index==1 && mo->index==12)	 CGoGNout << *it << " ; ";

			addElementToVector<Obstacle*>(obstvect[*it],o);
			mo->addGeneralCell (*it);

	}
//	if(o->index==1 && mo->index==12)	CGoGNout <<CGoGNendl;
	addObstAsNeighbor(o, mo->belonging_cells[n], &(mo->neighbor_cells[n]));

	for (std::vector<Dart>::iterator it = mo->neighbor_cells[n].begin(); it != mo->neighbor_cells[n].end(); ++it)
	{
		//		 CGoGNout <<"obstacle"<< o->p1 << "new cell : "<< *it << CGoGNendl;
		if (!map.isBoundaryMarked2(*it))
		{
			addElementToVector<Obstacle*>(neighborObstvect[*it],o);
		}
//		pushObstNeighborInCells(o, *it);
	}
}

void EnvMap::popObstacleInCells(Obstacle* o, int n)
{
	MovingObstacle * mo = o->mo;

	assert(map.getCurrentLevel() == map.getMaxLevel()) ;

	if (mo != NULL)
	{
//		if (mo->index==12 && o->index ==1) CGoGNout<<"pop : "<<CGoGNendl;
		for (std::vector<Dart>::iterator it = mo->belonging_cells[n].begin();	it != mo->belonging_cells[n].end(); ++it)
		{
//			if (mo->index==12 && o->index ==1) CGoGNout<<*it<< " ; ";
			removeElementFromVector<Obstacle*>(obstvect[*it], o) ;
			mo->removeGeneralCell (*it);
		}
//		if (mo->index==12 && o->index ==1) CGoGNout<<CGoGNendl<<CGoGNendl;
		for (std::vector<Dart>::iterator it = mo->neighbor_cells[n].begin(); it != mo->neighbor_cells[n].end(); ++it)
		{
			if (!map.isBoundaryMarked2(*it))
			{
				removeElementFromVector<Obstacle*>(neighborObstvect[*it], o) ;
			}
		}
		mo->belonging_cells[n].clear();
		mo->neighbor_cells[n].clear();
	}
}

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::refine()
{
//	CGoGNout<<"refine"<<CGoGNendl;
	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é
				float minSizeSq = Agent::neighborDistSq_;

				fLevel = map.faceLevel(old) ;
				map.setCurrentLevel(fLevel) ;
				PFP::VEC3 fCenter = Algo::Surface::Geometry::faceCentroid<PFP>(map, old, position) ;
				Dart fd = old ;
				do
				{
					PFP::VEC3& p = position[fd] ;
					PFP::VEC3 edge = Algo::Surface::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)
					this->popObstacleInCells(*ait, (*ait)->index) ;
				for (PFP::OBSTACLEVECT::iterator ait = oldNeighborObst.begin();ait != oldNeighborObst.end(); ++ait)
					this->popObstacleInCells(*ait, (*ait)->index) ;
				neighborAgentvect[old].clear() ;

				map.setCurrentLevel(fLevel) ;
				Algo::Surface::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) ;
				map.setCurrentLevel(fLevel+1) ;
				if (degree == 3)
				{
					Dart centerFace = map.phi2(map.phi1(old)) ;