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