env_map.cpp 40.9 KB
Newer Older
Pierre Kraemer's avatar
Pierre Kraemer committed
1 2 3
#include "env_map.h"
#include "utils.h"
#include "agent.h"
Pierre Kraemer's avatar
Pierre Kraemer committed
4
#include "obstacle.h"
5 6
#include "moving_obstacle.h"
#include "simulator.h"
Pierre Kraemer's avatar
Pierre Kraemer committed
7

Pierre Kraemer's avatar
Pierre Kraemer committed
8 9
#include "Geometry/inclusion.h"
#include "Algo/MovingObjects/particle_cell_2D_memo.h"
Pierre Kraemer's avatar
Pierre Kraemer committed
10
#include "Algo/Modelisation/subdivision.h"
Jund Thomas's avatar
Jund Thomas committed
11
#include "Algo/Modelisation/triangulation.h"
Pierre Kraemer's avatar
Pierre Kraemer committed
12
#include "Algo/Geometry/normal.h"
Thomas's avatar
Thomas committed
13 14
#include "Algo/Import/importSvg.h"
#include "Algo/BooleanOperator/mergeVertices.h"
Pierre Kraemer's avatar
Pierre Kraemer committed
15

Thomas Jund's avatar
Thomas Jund committed
16 17
#include "Topology/generic/mapBrowser.h"

18
#include "env_generator.h"
19
#include <dirent.h>
20

21
using namespace CGoGN ;
Pierre Kraemer's avatar
Pierre Kraemer committed
22

Pierre Kraemer's avatar
Pierre Kraemer committed
23
EnvMap::EnvMap() :
David Cazier's avatar
David Cazier committed
24
	obstacleDistance(Agent::range_),
David Cazier's avatar
David Cazier committed
25 26 27 28 29
	obstacleMarkS(mapScenary),
	buildingMarkS(mapScenary),
	obstacleMark(map),
	buildingMark(map),
	pedWayMark(map),
30

Pierre Kraemer's avatar
Pierre Kraemer committed
31
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
32 33
	refineMark(map),
	coarsenMark(map)
Pierre Kraemer's avatar
Pierre Kraemer committed
34
#else
David Cazier's avatar
David Cazier committed
35 36 37
	ht_agents(1024),
	ht_neighbor_agents(1024),
	ht_obstacles(1024)
Pierre Kraemer's avatar
Pierre Kraemer committed
38
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
39
{
David Cazier's avatar
David Cazier committed
40 41
	position = map.addAttribute<VEC3, VERTEX>("position") ;
	normal = map.addAttribute<VEC3, VERTEX>("normal") ;
Pierre Kraemer's avatar
Pierre Kraemer committed
42

Jund Thomas's avatar
Jund Thomas committed
43
	positionScenary = mapScenary.addAttribute<VEC3, VERTEX>("position") ;
Jund Thomas's avatar
Jund Thomas committed
44
	normalScenary = mapScenary.addAttribute<VEC3, VERTEX>("normal") ;
Jund Thomas's avatar
Jund Thomas committed
45

Pierre Kraemer's avatar
Pierre Kraemer committed
46
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
47 48 49 50 51
	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") ;
Thomas's avatar
Thomas committed
52

53 54 55 56 57 58 59 60
	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 ;
David Cazier's avatar
David Cazier committed
61
	VEC3 bottomLeft(-width / 2, -height / 2, 0.0f) ;
62
	VEC3 topRight(width / 2, height / 2, 0.0f) ;
Thomas Jund's avatar
Thomas Jund committed
63 64
	
	this->config = config;
65 66 67 68 69 70 71

	geometry.reset() ;
	geometry.addPoint(bottomLeft) ;
	geometry.addPoint(topRight) ;
	minCellSize = minSize ;
	maxCellSize = maxSize ;
	std::cout << " - Geometry : " << geometry ;
pitiot's avatar
pitiot committed
72
	std::cout << " - Geometry size : " << geometry.size(0)<<" x "<<geometry.size(1) ;
73 74
	std::cout << " - Cell size between : " << minSize << " and " << maxSize << std::endl ;
#ifdef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
75
	std::cout << " - Table de hachage : " << agentGridSize(0) << " x " << agentGridSize(1) << std::endl ;
76 77
#endif

David Cazier's avatar
David Cazier committed
78 79
	switch (config)
	{
David Cazier's avatar
David Cazier committed
80
		case 0 :
pitiot's avatar
merging  
pitiot committed
81
//			CityGenerator::generateGrid<PFP>(*this) ;
pitiot's avatar
pitiot committed
82 83 84
			CityGenerator::generateGrid<PFP>(*this) ;
//			CityGenerator::generateTrianGrid<PFP>(*this,obstacleMark, buildingMark);
//			CityGenerator::generateCity<PFP>(*this,0,500.0f) ;
David Cazier's avatar
David Cazier committed
85 86
			break ;
		case 1 :
pitiot's avatar
pitiot committed
87 88
//			CityGenerator::generateGrid<PFP>(*this) ;
			CityGenerator::generateTrianGrid<PFP>(*this,obstacleMark, buildingMark);
David Cazier's avatar
David Cazier committed
89 90
			break ;
		case 2 :
pitiot's avatar
pitiot committed
91 92
//			CityGenerator::generateGrid<PFP>(*this) ;
			CityGenerator::generateTrianGrid<PFP>(*this,obstacleMark, buildingMark);
David Cazier's avatar
David Cazier committed
93 94
			// CityGenerator::generateMall<PFP>(map, position, obstacleMark, buildingMark, sideSize);
			break ;
Jund Thomas's avatar
Jund Thomas committed
95
		case 3 :
pitiot's avatar
pitiot committed
96 97
//			CityGenerator::generateGrid<PFP>(*this) ;
			CityGenerator::generateTrianGrid<PFP>(*this,obstacleMark, buildingMark);
pitiot's avatar
pitiot committed
98
//			CityGenerator::generateCity<PFP>(*this,10,500.0f) ;
Jund Thomas's avatar
Jund Thomas committed
99 100
			break ;
		case 4 :
pitiot's avatar
pitiot committed
101 102 103
			CityGenerator::generateTrianGrid<PFP>(*this,obstacleMark, buildingMark);

//			CityGenerator::generateGrid<PFP>(*this) ;
Jund Thomas's avatar
Jund Thomas committed
104 105 106
			break ;
		case 5 :
		{
Jund Thomas's avatar
Jund Thomas committed
107
			std::cout << "?" << std::endl;
Jund Thomas's avatar
Jund Thomas committed
108
//			std::string filename = "./svg/planet.svg" ;
109
			std::string filename = "./svg/mapRoads.svg" ;
Jund Thomas's avatar
Jund Thomas committed
110
//			std::string filename = "./svg/simpleCross.svg" ;
111
			Algo::Surface::Import::importSVG<PFP>(map, filename, position, obstacleMark, buildingMark) ;
Jund Thomas's avatar
Jund Thomas committed
112

Thomas Jund's avatar
Thomas Jund committed
113 114
			std::string filename2 = "./svg/mapBuild.svg" ;
			Algo::Surface::Import::importSVG<PFP>(mapScenary, filename2, positionScenary, obstacleMarkS, buildingMarkS) ;
Jund Thomas's avatar
Jund Thomas committed
115

116 117
			Geom::BoundingBox<PFP::VEC3> bb2 ;
			bb = Algo::Geometry::computeBoundingBox<PFP>(map, position) ;
Thomas Jund's avatar
Thomas Jund committed
118
			bb2 = Algo::Geometry::computeBoundingBox<PFP>(mapScenary, positionScenary) ;
119 120
			bb.addPoint(bb2.min());
			bb.addPoint(bb2.max());
Jund Thomas's avatar
Jund Thomas committed
121

122 123 124 125
			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);
Jund Thomas's avatar
Jund Thomas committed
126 127

			std::vector<Dart> v;
Jund Thomas's avatar
Jund Thomas committed
128 129 130
			TraversorF<PFP::MAP> tF(mapScenary);
			for(Dart d = tF.begin() ; d != tF.end() ; d = tF.next())
			{
Jund Thomas's avatar
Jund Thomas committed
131 132 133 134 135 136 137 138 139
				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;

Jund Thomas's avatar
Jund Thomas committed
140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177
				//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)
				{
178
					if(Algo::Surface::Geometry::convexFaceArea<PFP>(mapScenary, d, positionScenary)<20.0f) //if big enough create building with "stairs"
Jund Thomas's avatar
Jund Thomas committed
179 180 181 182 183 184 185 186 187 188 189
						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
190
			Algo::Surface::Modelisation::EarTriangulation<PFP> et(mapScenary);
Jund Thomas's avatar
Jund Thomas committed
191 192
			et.triangule();

193 194
//			Algo::Modelisation::EarTriangulation<PFP> et2(map);
//			et2.triangule();
Jund Thomas's avatar
Jund Thomas committed
195

Jund Thomas's avatar
Jund Thomas committed
196
			//subdivision to create footpath
Sylvain Thery's avatar
Sylvain Thery committed
197
			MapBrowserSelector mbs(map, SelectorDartNoBoundary<PFP::MAP>(map));
Thomas Jund's avatar
Thomas Jund committed
198
			map.setBrowser(&mbs);
199
			Algo::Surface::Modelisation::TwoNPlusOneSubdivision<PFP,PFP::TVEC3, VEC3>(map,position,1.0f/5.0f);
Sylvain Thery's avatar
Sylvain Thery committed
200
			map.setBrowser(NULL);
201 202
			markPedWay();

203
			scale(20.0f);
David Cazier's avatar
David Cazier committed
204
		}
Jund Thomas's avatar
Jund Thomas committed
205
		break;
206 207
		case 6:
			CityGenerator::generatePlanet<PFP>(*this);
pitiot's avatar
pitiot committed
208
			CityGenerator::generateCity<PFP>(*this,30, 1000.0f, 20.0f) ;
pitiot's avatar
merging  
pitiot committed
209 210 211 212 213 214 215 216
			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);
pitiot's avatar
final  
pitiot committed
217
			CityGenerator::generateCity<PFP>(*this,300, 150.0f, 15.0f) ;
pitiot's avatar
merging  
pitiot committed
218 219 220 221 222
			break;
		case 8 :

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

Thomas Jund's avatar
Thomas Jund committed
223
			CityGenerator::importObj<PFP>(*this, "./meshRessources/knot3CenteredSubSmooth.obj");
pitiot's avatar
merging  
pitiot committed
224 225

			scale(150.0f);
pitiot's avatar
plop  
pitiot committed
226
//			CityGenerator::generateCity<PFP>(*this,1000, 150.0f,5.0f) ;
pitiot's avatar
merging  
pitiot committed
227 228 229
			break;
		case 9 :
			CityGenerator::importObj<PFP>(*this, "./meshRessources/simpleSlugSmoothTest.obj");
Thomas Jund's avatar
Thomas Jund committed
230
// 			CityGenerator::importObj<PFP>(*this, "./meshRessources/LimSol.obj");
pitiot's avatar
merging  
pitiot committed
231
			scale(150.0f);
Thomas Jund's avatar
Thomas Jund committed
232
// 			CityGenerator::generateCity<PFP>(*this,200, 150.0f,15.0f) ;
233
			break;
234 235 236 237 238 239 240 241
	}

//	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() ;
pitiot's avatar
pitiot committed
242
	registerObstaclesInFaces();
David Cazier's avatar
David Cazier committed
243
	// TODO Check registerWallInFaces();
Thomas Jund's avatar
Thomas Jund committed
244
//	registerWallInFaces() ;
245 246
//	subdivideAllToMaxLevel();

247
	for (unsigned int i = subdivisableFace.begin(); i < subdivisableFace.end(); subdivisableFace.next(i))
248
		subdivisableFace[i].first = false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
249
#endif
250

Thomas Jund's avatar
Thomas Jund committed
251 252 253 254 255 256 257 258 259 260 261
	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) ;
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
262 263
}

264 265 266
void EnvMap::initGL()
{
#ifdef EXPORTING3
267 268
	std::vector<std::string> filenames;
	std::vector<std::string> texturenames;
Thomas Jund's avatar
Thomas Jund committed
269 270 271 272 273 274
	
	std::ostringstream tmp;
	tmp << "./meshRessources/scenario"  << config << "/" ;
			
	std::string dir = tmp.str();
	
275 276 277
	DIR *dp;
	struct dirent *dirp;
	if((dp  = opendir(dir.c_str())) == NULL)
278
	{
279 280
		cout << "Error(" << errno << ") opening " << dir << endl;
		return;
281 282
	}

283 284 285 286 287 288 289
	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;
		}
290

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

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

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

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

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


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

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

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

318 319 320 321 322 323 324 325
		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);

pitiot's avatar
pitiot committed
326
//		TraversorV<PFP2::MAP> tV(*nmap);
pitiot's avatar
merging  
pitiot committed
327

pitiot's avatar
merging  
pitiot committed
328 329 330 331 332
//		for(Dart d = tV.begin() ; d != tV.end() ; d = tV.next())
////		{
////			position_Export[d] *= 100.0f;
////			position_Export[d] += VEC3(2000,0,0);
////		}
333 334 335 336 337 338



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

340 341 342
		Utils::VBO * texcoordVBO = new Utils::VBO();
		Utils::VBO * positionVBO = new Utils::VBO();
		Utils::VBO * normalVBO = new Utils::VBO();
343

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

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

351 352 353 354 355 356 357 358
		texture->setWrapping(GL_CLAMP_TO_EDGE);

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

359 360 361 362 363 364 365
		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));

366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394
		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;
395 396 397 398 399 400 401 402
#endif
}

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

403 404
	glEnable(GL_POLYGON_OFFSET_FILL) ;

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

409 410 411 412 413 414 415 416
		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();
	}
417 418 419

	glDisable(GL_POLYGON_OFFSET_FILL) ;

420 421 422 423 424 425 426 427 428 429 430 431 432 433
//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
}

434 435 436 437 438 439 440 441 442 443 444 445
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;

}

pitiot's avatar
up  
pitiot committed
446 447 448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474
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;
}

475 476
void EnvMap::markPedWay()
{
477
	//split all pedway with 3 roads intersection (to avoid spikes)
David Cazier's avatar
David Cazier committed
478
	CellMarker<FACE> treat(map) ;
David Cazier's avatar
David Cazier committed
479 480 481 482
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!treat.isMarked(d))
		{
483
			treat.mark(d) ;
484 485
			//intern face for intersection
			if(map.faceDegree(d)==3)
David Cazier's avatar
David Cazier committed
486
			{
487
				Dart dd = d;
David Cazier's avatar
David Cazier committed
488 489
				do
				{
490 491 492 493 494 495 496 497
					//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);
			}
498 499 500
		}
	}

501
	treat.unmarkAll();
David Cazier's avatar
David Cazier committed
502 503 504 505
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!treat.isMarked(d))
		{
506
			treat.mark(d) ;
507
			//check around the face
508
			Dart dd = d ;
David Cazier's avatar
David Cazier committed
509 510
			do
			{
511 512 513
				//if any vertex
				Dart ddd = dd ;
				do
David Cazier's avatar
David Cazier committed
514
				{
515 516 517 518 519 520 521 522 523
					//is an obstacle
					if (obstacleMark.isMarked(ddd))
					{
						//then mark as pedway
						pedWayMark.mark(d) ;
						break;
					}
					ddd = map.alpha1(ddd) ;
				} while(ddd != dd);
524

525 526
				dd = map.phi1(dd);
			} while (dd != d);
527 528 529 530
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
531 532
unsigned int EnvMap::mapMemoryCost()
{
David Cazier's avatar
David Cazier committed
533 534 535 536
	return (map.getAttributeContainer<DART>()).memorySize()
	    + (map.getAttributeContainer<VERTEX>()).memorySize()
	    + (map.getAttributeContainer<EDGE>()).memorySize()
	    + (map.getAttributeContainer<FACE>()).memorySize() ;
Thomas's avatar
Thomas committed
537 538
}

Pierre Kraemer's avatar
Pierre Kraemer committed
539
#ifndef SPATIAL_HASHING
Thomas's avatar
Thomas committed
540 541
void EnvMap::subdivideAllToMaxLevel()
{
542
	bool subdiv ;
David Cazier's avatar
David Cazier committed
543 544
	do
	{
545
		subdiv = false ;
Thomas's avatar
Thomas committed
546
		{
David Cazier's avatar
David Cazier committed
547
			CellMarker<FACE> subd(map) ;
David Cazier's avatar
David Cazier committed
548 549 550 551
			for (Dart d = map.begin(); d != map.end(); map.next(d))
			{
				if (!subd.isMarked(d))
				{
552
					subd.mark(d) ;
David Cazier's avatar
David Cazier committed
553 554
					if (!buildingMark.isMarked(d))
					{
Thomas's avatar
Thomas committed
555 556
						//check if subdivision is authorized
						float minDistSq = Agent::neighborDistSq_ ;
557
						bool subdivisable = true ;
Thomas's avatar
Thomas committed
558 559 560
						Dart old = map.faceOldestDart(d) ;
						unsigned int fLevel = map.faceLevel(old) ;
						map.setCurrentLevel(fLevel) ;
561
						PFP::VEC3 fCenter = Algo::Surface::Geometry::faceCentroid<PFP>(map, old, position) ;
Thomas's avatar
Thomas committed
562
						Dart fd = old ;
David Cazier's avatar
David Cazier committed
563 564
						do
						{
Thomas's avatar
Thomas committed
565
							PFP::VEC3& p = position[fd] ;
566
							PFP::VEC3 edge = Algo::Surface::Geometry::vectorOutOfDart<PFP>(map, fd,
David Cazier's avatar
David Cazier committed
567
							                                                      position) ;
568
							PFP::VEC3 proj = fCenter
David Cazier's avatar
David Cazier committed
569 570 571
							    - (p + (edge * (fCenter - p) / edge.norm2()) * edge) ;
							if (proj.norm2() < minDistSq)
							{
Thomas's avatar
Thomas committed
572 573 574 575
								subdivisable = false ;
								break ;
							}
							fd = map.phi1(fd) ;
576
						} while (fd != old) ;
Thomas's avatar
Thomas committed
577

David Cazier's avatar
David Cazier committed
578 579
						if (subdivisable)
						{
Thomas's avatar
Thomas committed
580
							map.setCurrentLevel(fLevel) ;
581
							Algo::Surface::IHM::subdivideFace<PFP>(map, old, position) ;
582
							subdiv = true ;
Thomas's avatar
Thomas committed
583 584 585 586 587 588
						}
						map.setCurrentLevel(map.getMaxLevel()) ;
					}
				}
			}
		}
589
	} while (subdiv) ;
Thomas's avatar
Thomas committed
590 591 592 593
}

void EnvMap::subdivideToProperLevel()
{
594
	bool subdiv ;
David Cazier's avatar
David Cazier committed
595 596
	do
	{
597
		subdiv = false ;
Thomas's avatar
Thomas committed
598
		{
David Cazier's avatar
David Cazier committed
599
			CellMarker<FACE> subd(map) ;
David Cazier's avatar
David Cazier committed
600 601 602 603
			for (Dart d = map.begin(); d != map.end(); map.next(d))
			{
				if (!subd.isMarked(d))
				{
604
					subd.mark(d) ;
David Cazier's avatar
David Cazier committed
605 606
					if (!refineMark.isMarked(d) && agentvect[d].size() > nbAgentsToSubdivide)
					{
607
						std::pair<bool, bool>& sf = subdivisableFace[d] ;
David Cazier's avatar
David Cazier committed
608 609
						if (sf.first == false || (sf.first == true && sf.second))
						{
610 611 612
							subdiv = true ;
							refineMark.mark(d) ;
							refineCandidate.push_back(d) ;
Thomas's avatar
Thomas committed
613 614 615 616
						}
					}
				}
			}
617
			subd.unmarkAll() ;
Thomas's avatar
Thomas committed
618
		}
619 620 621 622
		updateMap() ;
		refineCandidate.clear() ;
		map.setCurrentLevel(map.getMaxLevel()) ;
	} while (subdiv) ;
Thomas's avatar
Thomas committed
623 624
}

Pierre Kraemer's avatar
Pierre Kraemer committed
625 626
Dart EnvMap::getBelongingCell(const PFP::VEC3& pos)
{
pitiot's avatar
...  
pitiot committed
627
//	assert(map.getCurrentLevel() == map.getMaxLevel()) ;
pitiot's avatar
pitiot committed
628

David Cazier's avatar
David Cazier committed
629
	CellMarkerStore<FACE> m(map) ;
David Cazier's avatar
David Cazier committed
630 631 632 633
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!m.isMarked(d))
		{
634
			m.mark(d) ;
635 636
			if (!buildingMark.isMarked(d) && Algo::Surface::Geometry::isPointInConvexFace2D<PFP>(map, d, position, pos, true))
				return d ;
637
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
638 639
	}

640
	std::cout << "ERROR : pos not in map for getBelongingCell " << pos << std::endl ;
641
	return map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
642
}
643 644 645 646 647 648 649 650 651 652 653

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) ;
pitiot's avatar
up  
pitiot committed
654 655
//			if (!buildingMark.isMarked(d) && Algo::Surface::Geometry::isPointInConvexFace<PFP>(map, d, position, pos, true))
			if (!buildingMark.isMarked(d) && isInsideFace3D(pos,d) )
656 657 658 659 660 661
			{
					return d ;
			}
		}
	}

pitiot's avatar
pitiot committed
662
	std::cout << "ERROR : pos not in map for getBelongingCellonSurface " << pos << std::endl ;
663 664 665
	return map.begin() ;
}

Pierre Kraemer's avatar
Pierre Kraemer committed
666
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
667

Pierre Kraemer's avatar
Pierre Kraemer committed
668
#ifndef SPATIAL_HASHING
Pierre Kraemer's avatar
Pierre Kraemer committed
669 670
void EnvMap::foreach_neighborFace(Dart d, FunctorType& f)
{
671
	Dart dd = d ;
David Cazier's avatar
David Cazier committed
672 673
	do
	{
674
		Dart ddd = map.alpha1(map.alpha1(dd)) ;
David Cazier's avatar
David Cazier committed
675 676
		while (ddd != dd)
		{
677 678
			f(ddd) ;
			ddd = map.alpha1(ddd) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
679
		}
680 681
		dd = map.phi1(dd) ;
	} while (dd != d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
682 683
}

David Cazier's avatar
David Cazier committed
684 685 686 687 688 689 690 691 692 693 694 695 696 697 698 699 700
// 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
			{
701
				if (obstacleMark.isMarked(dd) && map.isBoundaryMarked2(dd))
David Cazier's avatar
David Cazier committed
702 703 704
				{
					Dart dd2 = map.phi2(dd) ;
					Dart next = map.phi1(dd2) ;
pitiot's avatar
pitiot committed
705 706

					Obstacle* o = new Obstacle(position[next], position[dd2], NULL, 0) ;
David Cazier's avatar
David Cazier committed
707 708 709 710 711 712 713 714 715 716 717 718 719 720 721 722 723 724 725 726 727 728
					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);
		}
	}

}

Pierre Kraemer's avatar
Pierre Kraemer committed
729 730
void EnvMap::registerObstaclesInFaces()
{
David Cazier's avatar
David Cazier committed
731
	CellMarker<FACE> m(map) ;
David Cazier's avatar
David Cazier committed
732 733 734 735
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!m.isMarked(d))
		{
736
			m.mark(d) ;
David Cazier's avatar
David Cazier committed
737 738
			if (!buildingMark.isMarked(d))
			{
739
				Dart dd = d ;
740 741 742 743

				//retrieve all obstacles within its one-ring

				//first : test all edges of the face of d
David Cazier's avatar
David Cazier committed
744 745 746 747
				do
				{
					if (obstacleMark.isMarked(dd))
					{
748 749
						Dart dd2 = map.phi2(dd) ;
						Dart next = map.phi1(dd2) ;
pitiot's avatar
pitiot committed
750

751
						Obstacle* o = new Obstacle(position[dd2], position[next],
David Cazier's avatar
David Cazier committed
752
						                           NULL, 0) ;
753
						obstvect[d].push_back(o) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
754
					}
755 756
					dd = map.phi1(dd) ;
				} while (dd != d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
757

758
				//second : test all edges of neighboring faces
David Cazier's avatar
David Cazier committed
759 760
				do
				{
761
					Dart ddd = map.alpha1(map.alpha1(dd)) ;
David Cazier's avatar
David Cazier committed
762 763
					while (ddd != dd)
					{
David Cazier's avatar
David Cazier committed
764
						// TODO Check effet de bord
David Cazier's avatar
David Cazier committed
765 766
						if (!buildingMark.isMarked(ddd)) addNeighborObstacles(
						    obstvect[d], ddd, ddd == map.alpha_1(dd)) ;
767
						ddd = map.alpha1(ddd) ;
768
					}
769 770
					dd = map.phi1(dd) ;
				} while (dd != d) ;
771 772 773
			}
		}
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
774 775 776 777
}

void EnvMap::addNeighborObstacles(PFP::OBSTACLES& obst, Dart d, bool edgeNeighbor)
{
778
	Dart stop ;
David Cazier's avatar
David Cazier committed
779 780 781 782
	if (edgeNeighbor)
		stop = map.phi_1(map.phi_1(map.phi_1(d))) ;
	else
		stop = map.phi_1(map.phi_1(d)) ;
783 784

	Dart dd = d ;
David Cazier's avatar
David Cazier committed
785 786 787 788
	do
	{
		if (obstacleMark.isMarked(dd))
		{
789 790 791 792 793 794 795 796 797 798
//			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
//			{
799 800
			Dart dd2 = map.phi2(dd) ;
			Dart next = map.phi1(dd2) ;
pitiot's avatar
pitiot committed
801 802

			Obstacle* o = new Obstacle(position[dd2], position[next],  NULL, 0) ;
803
			obst.push_back(o) ;
804
//			}
Pierre Kraemer's avatar
Pierre Kraemer committed
805
		}
806 807
		dd = map.phi1(dd) ;
	} while (dd != stop) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
808 809
}

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

pitiot's avatar
pitiot committed
817 818
		VEC3 p1 = o->p1;
		VEC3 p2 =  o->p2;
819 820 821 822

		Dart d1=NIL;
		Dart d2=NIL;
		std::vector<Dart> memo;
pitiot's avatar
pitiot committed
823
		d1=mo->parts_[n]->d;
pitiot's avatar
pitiot committed
824

pitiot's avatar
pitiot committed
825
		memo = mo->getMemoCross(p1,p2,d1,d2);
826 827 828 829 830 831 832

		if(map.sameFace(d1,d2))
		{
			pushObstacleInOneRingCells(o , d1 , n);
		}
		else
		{
pitiot's avatar
pitiot committed
833

834 835 836 837 838
			pushObstacleInCells(o, n, memo);
		}
	}
}

Thomas Jund's avatar
Thomas Jund committed
839
Dart EnvMap::popAndPushObstacleInCells(Obstacle* o, int n)// maj de l'enregistrement
840 841
{
	MovingObstacle * mo = o->mo;
pitiot's avatar
pitiot committed
842 843
	VEC3 p1 = o->p1;
	VEC3 p2 = o->p2;
844 845 846 847

	Dart d1=NIL;
	Dart d2=NIL;
	std::vector<Dart> memo;
pitiot's avatar
pitiot committed
848
	d1=mo->parts_[n]->d;
849 850 851 852 853
//	bool modif=false;

//	if(p1->crossCell != CGoGN::Algo::MovingObjects::NO_CROSS
//		|| p2->crossCell != CGoGN::Algo::MovingObjects::NO_CROSS)
//	{
pitiot's avatar
pitiot committed
854
		memo = mo->getMemoCross(p1,p2,d1,d2);
855 856 857 858 859 860 861 862 863 864
//		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;
//	}
pitiot's avatar
up  
pitiot committed
865
//	if (mo->index==12 && o->index ==1) CGoGNout<<"popAndPush :"<<CGoGNendl;
866 867 868 869 870 871 872 873 874 875 876 877 878
//	if(modif)
	{
		popObstacleInCells(o, n);

		if(map.sameFace(d1,d2))
		{
			pushObstacleInOneRingCells(o , d1 , n);
		}
		else
		{
			pushObstacleInCells(o, n, memo);
		}
	}
Thomas Jund's avatar
Thomas Jund committed
879 880

	return d1;
881 882 883 884 885 886 887 888 889 890
}

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

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

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

pitiot's avatar
pitiot committed
891
//	mo->belonging_cells[n].clear();
892
	mo->belonging_cells[n].push_back(d);
pitiot's avatar
pitiot committed
893
	mo->addGeneralCell (d);
pitiot's avatar
pitiot committed
894
//	mo->neighbor_cells[n].clear();
pitiot's avatar
up  
pitiot committed
895
//	if(o->index==1 && mo->index==12)	 CGoGNout <<"push : "<< d << CGoGNendl;
896 897 898 899 900 901
	Dart dd = d;
	do
	{
		Dart ddd = map.alpha1(map.alpha1(dd));
		while(ddd != dd)
		{
902 903 904 905
			if (!map.isBoundaryMarked2(ddd))
			{
				pushObstNeighborInCells(o, ddd);
			}
906 907 908 909 910 911 912 913 914 915 916 917 918 919 920 921 922

			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;

pitiot's avatar
pitiot committed
923 924
//	mo->belonging_cells[n].clear();
//	mo->neighbor_cells[n].clear();
925
	mo->belonging_cells[n] = memo_cross;
pitiot's avatar
up  
pitiot committed
926
//	if (mo->index==12 && o->index ==1) CGoGNout<<"push : "<<CGoGNendl;
927 928
	for (std::vector<Dart>::iterator it = mo->belonging_cells[n].begin();	it != mo->belonging_cells[n].end(); ++it)
	{
pitiot's avatar
up  
pitiot committed
929
//			if(o->index==1 && mo->index==12)	 CGoGNout << *it << " ; ";
930 931

			addElementToVector<Obstacle*>(obstvect[*it],o);
pitiot's avatar
pitiot committed
932
			mo->addGeneralCell (*it);
933

934
	}
pitiot's avatar
up  
pitiot committed
935
//	if(o->index==1 && mo->index==12)	CGoGNout <<CGoGNendl;
936 937 938 939 940
	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;
941 942 943 944
		if (!map.isBoundaryMarked2(*it))
		{
			addElementToVector<Obstacle*>(neighborObstvect[*it],o);
		}
Jund Thomas's avatar
Jund Thomas committed
945
//		pushObstNeighborInCells(o, *it);
946 947 948 949 950 951 952 953 954 955 956
	}
}

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

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

	if (mo != NULL)
	{
pitiot's avatar
up  
pitiot committed
957
//		if (mo->index==12 && o->index ==1) CGoGNout<<"pop : "<<CGoGNendl;
958 959
		for (std::vector<Dart>::iterator it = mo->belonging_cells[n].begin();	it != mo->belonging_cells[n].end(); ++it)
		{
pitiot's avatar
up  
pitiot committed
960
//			if (mo->index==12 && o->index ==1) CGoGNout<<*it<< " ; ";
961
			removeElementFromVector<Obstacle*>(obstvect[*it], o) ;
pitiot's avatar
pitiot committed
962
			mo->removeGeneralCell (*it);
963
		}
pitiot's avatar
up  
pitiot committed
964
//		if (mo->index==12 && o->index ==1) CGoGNout<<CGoGNendl<<CGoGNendl;
965 966
		for (std::vector<Dart>::iterator it = mo->neighbor_cells[n].begin(); it != mo->neighbor_cells[n].end(); ++it)
		{
967 968 969 970
			if (!map.isBoundaryMarked2(*it))
			{
				removeElementFromVector<Obstacle*>(neighborObstvect[*it], o) ;
			}
971
		}
pitiot's avatar
pitiot committed
972 973
		mo->belonging_cells[n].clear();
		mo->neighbor_cells[n].clear();
974 975 976
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
977 978
void EnvMap::agentChangeFace(Agent* agent, Dart oldFace)
{
979
	Dart newFace = agent->part_.d ;
980

981 982
	popAgentInCells(agent, oldFace) ;
	pushAgentInCells(agent, newFace) ;
983

David Cazier's avatar
David Cazier committed
984 985
	if (!refineMark.isMarked(newFace) && agentvect[newFace].size() > nbAgentsToSubdivide)
	{
986
		std::pair<bool, bool>& sf = subdivisableFace[newFace] ;
David Cazier's avatar
David Cazier committed
987 988
		if (sf.first == false || (sf.first == true && sf.second))
		{
989 990
			refineMark.mark(newFace) ;
			refineCandidate.push_back(newFace) ;
991 992
		}
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
993

David Cazier's avatar
David Cazier committed
994 995 996
	if (!coarsenMark.isMarked(oldFace) && !refineMark.isMarked(oldFace)
	    && 4 * agentvect[oldFace].size() < nbAgentsToSimplify)
	{
997 998
		coarsenMark.mark(oldFace) ;
		coarsenCandidate.push_back(map.faceOldestDart(oldFace)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
999
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
1000 1001
}

1002
void EnvMap::refine()
Pierre Kraemer's avatar
Pierre Kraemer committed
1003
{
pitiot's avatar
up  
pitiot committed
1004
//	CGoGNout<<"refine"<<CGoGNendl;
1005
	for (std::vector<Dart>::iterator it = refineCandidate.begin(); it != refineCandidate.end(); ++it)
David Cazier's avatar
David Cazier committed
1006 1007
	{
		Dart d = (*it) ;
1008
		refineMark.unmark(d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
1009

1010
		//if the number of agents is big enough
David Cazier's avatar
David Cazier committed
1011 1012
		if (agentvect[d].size() > nbAgentsToSubdivide)
		{
Thomas's avatar
Thomas committed
1013
			int fLevel = -1 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
1014 1015
			Dart old = map.faceOldestDart(d) ;

1016
			bool subdivisable = true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
1017

1018
			//check if faces resulting from a subdivision are big enough
1019
			std::pair<bool, bool>& sf = subdivisableFace[old] ;
David Cazier's avatar
David Cazier committed
1020 1021 1022 1023
			if (sf.first == true)
				subdivisable = sf.second ;
			else
			{
1024 1025
//				float minSizeSq = minCellSize * minCellSize ; // diametre de vision de l'agent au carré
				float minSizeSq = Agent::neighborDistSq_;
1026

Pierre Kraemer's avatar
Pierre Kraemer committed
1027
				fLevel = map.faceLevel(old) ;
1028
				map.setCurrentLevel(fLevel) ;
1029
				PFP::VEC3 fCenter = Algo::Surface::Geometry::faceCentroid<PFP>(map, old, position) ;
1030
				Dart fd = old ;
David Cazier's avatar
David Cazier committed
1031 1032
				do
				{
Pierre Kraemer's avatar
Pierre Kraemer committed
1033
					PFP::VEC3& p = position[fd] ;
1034
					PFP::VEC3 edge = Algo::Surface::Geometry::vectorOutOfDart<PFP>(map, fd, position) ;
David Cazier's avatar
David Cazier committed
1035 1036 1037
					PFP::VEC3 proj = fCenter - (p + (edge * (fCenter - p) / edge.norm2()) * edge) ;
					if (proj.norm2() < minSizeSq)
					{
1038 1039
						subdivisable = false ;
						break ;
Pierre Kraemer's avatar
Pierre Kraemer committed
1040
					}
1041
					fd = map.phi1(fd) ;
1042
				} while (fd != old) ;
1043 1044
				map.setCurrentLevel(map.getMaxLevel()) ;

Pierre Kraemer's avatar
Pierre Kraemer committed
1045 1046
				sf.first = true ;
				sf.second = subdivisable ;
1047 1048
			}

David Cazier's avatar
David Cazier committed
1049 1050
			if (subdivisable)
			{
1051 1052
				if (fLevel == -1)
					fLevel = map.faceLevel(old) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
1053 1054

				sf.first = false ;
1055

1056
				PFP::AGENTS oldAgents(agentvect[old]) ;
David Cazier's avatar
David Cazier committed
1057 1058
				PFP::OBSTACLEVECT oldObst(obstvect[old]) ;
				PFP::OBSTACLEVECT oldNeighborObst(neighborObstvect[old]) ;
Thomas Jund's avatar
Thomas Jund committed
1059

David Cazier's avatar
David Cazier committed
1060
				for (PFP::AGENTS::iterator ait = oldAgents.begin(); ait != oldAgents.end(); ++ait)
Thomas Jund's avatar
Thomas Jund committed
1061
				{
David Cazier's avatar
David Cazier committed
1062
					popAgentInCells(*ait, old) ;
Thomas Jund's avatar
Thomas Jund committed
1063
				}
David Cazier's avatar
David Cazier committed
1064
				for (PFP::OBSTACLEVECT::iterator ait = oldObst.begin(); ait != oldObst.end(); ++ait)
1065 1066 1067
					this->popObstacleInCells(*ait, (*ait)->index) ;
				for (PFP::OBSTACLEVECT::iterator ait = oldNeighborObst.begin();ait != oldNeighborObst.end(); ++ait)
					this->popObstacleInCells(*ait, (*ait)->index) ;
1068
				neighborAgentvect[old].clear() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
1069 1070

				map.setCurrentLevel(fLevel) ;
1071
				Algo::Surface::IHM::subdivideFace<PFP>(map, old, position) ;
David Cazier's avatar
David Cazier committed
1072
				CellMarkerStore<FACE> newF(map) ;
1073 1074
				unsigned int degree = 0 ;
				Dart dd = old ;
David Cazier's avatar
David Cazier committed
1075 1076
				do
				{
1077
					++degree ;
1078
					newF.mark(dd) ;
1079
					dd = map.phi1(dd) ;
1080
				} while (dd != old) ;
1081
				map.setCurrentLevel(fLevel+1) ;
David Cazier's avatar
David Cazier committed
1082 1083
				if (degree == 3)
				{
1084
					Dart centerFace = map.phi2(map.phi1(old)) ;