env_map.cpp 37.5 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 63 64 65 66 67 68 69
	VEC3 topRight(width / 2, height / 2, 0.0f) ;

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

David Cazier's avatar
David Cazier committed
76 77
	switch (config)
	{
David Cazier's avatar
David Cazier committed
78
		case 0 :
pitiot's avatar
merging  
pitiot committed
79
//			CityGenerator::generateGrid<PFP>(*this) ;
80
			CityGenerator::generateCity<PFP>(*this,0) ;
David Cazier's avatar
David Cazier committed
81 82
			break ;
		case 1 :
Thomas Jund's avatar
Thomas Jund committed
83 84
//			CityGenerator::generateGrid<PFP>(*this) ;
			CityGenerator::generateTrianGrid<PFP>(*this,obstacleMark, buildingMark);
David Cazier's avatar
David Cazier committed
85 86
			break ;
		case 2 :
pitiot's avatar
merging  
pitiot committed
87
			CityGenerator::generateGrid<PFP>(*this) ;
David Cazier's avatar
David Cazier committed
88 89
			// CityGenerator::generateMall<PFP>(map, position, obstacleMark, buildingMark, sideSize);
			break ;
Jund Thomas's avatar
Jund Thomas committed
90
		case 3 :
pitiot's avatar
merging  
pitiot committed
91
			CityGenerator::generateCity<PFP>(*this,10) ;
Jund Thomas's avatar
Jund Thomas committed
92 93 94 95 96 97
			break ;
		case 4 :
			CityGenerator::generatePlanet<PFP>(*this) ;
			break ;
		case 5 :
		{
Jund Thomas's avatar
Jund Thomas committed
98
			std::cout << "?" << std::endl;
Jund Thomas's avatar
Jund Thomas committed
99
//			std::string filename = "./svg/planet.svg" ;
100
			std::string filename = "./svg/mapRoads.svg" ;
Jund Thomas's avatar
Jund Thomas committed
101
//			std::string filename = "./svg/simpleCross.svg" ;
102
			Algo::Surface::Import::importSVG<PFP>(map, filename, position, obstacleMark, buildingMark) ;
Jund Thomas's avatar
Jund Thomas committed
103

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

107 108
			Geom::BoundingBox<PFP::VEC3> bb2 ;
			bb = Algo::Geometry::computeBoundingBox<PFP>(map, position) ;
Thomas Jund's avatar
Thomas Jund committed
109
			bb2 = Algo::Geometry::computeBoundingBox<PFP>(mapScenary, positionScenary) ;
110 111
			bb.addPoint(bb2.min());
			bb.addPoint(bb2.max());
Jund Thomas's avatar
Jund Thomas committed
112

113 114 115 116
			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
117 118

			std::vector<Dart> v;
Jund Thomas's avatar
Jund Thomas committed
119 120 121
			TraversorF<PFP::MAP> tF(mapScenary);
			for(Dart d = tF.begin() ; d != tF.end() ; d = tF.next())
			{
Jund Thomas's avatar
Jund Thomas committed
122 123 124 125 126 127 128 129 130
				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
131 132 133 134 135 136 137 138 139 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
				//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)
				{
169
					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
170 171 172 173 174 175 176 177 178 179 180
						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
181
			Algo::Surface::Modelisation::EarTriangulation<PFP> et(mapScenary);
Jund Thomas's avatar
Jund Thomas committed
182 183
			et.triangule();

184 185
//			Algo::Modelisation::EarTriangulation<PFP> et2(map);
//			et2.triangule();
Jund Thomas's avatar
Jund Thomas committed
186

Jund Thomas's avatar
Jund Thomas committed
187
			//subdivision to create footpath
Sylvain Thery's avatar
Sylvain Thery committed
188
			MapBrowserSelector mbs(map, SelectorDartNoBoundary<PFP::MAP>(map));
Thomas Jund's avatar
Thomas Jund committed
189
			map.setBrowser(&mbs);
190
			Algo::Surface::Modelisation::TwoNPlusOneSubdivision<PFP,PFP::TVEC3, VEC3>(map,position,1.0f/5.0f);
Sylvain Thery's avatar
Sylvain Thery committed
191
			map.setBrowser(NULL);
192 193
			markPedWay();

194
			scale(20.0f);
David Cazier's avatar
David Cazier committed
195
		}
Jund Thomas's avatar
Jund Thomas committed
196
		break;
197 198 199 200 201 202 203 204 205
	}

//	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();
David Cazier's avatar
David Cazier committed
206 207
	// TODO Check registerWallInFaces();
	registerWallInFaces() ;
208 209
//	subdivideAllToMaxLevel();

210
	for (unsigned int i = subdivisableFace.begin(); i < subdivisableFace.end(); subdivisableFace.next(i))
211
		subdivisableFace[i].first = false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
212
#endif
213

Thomas Jund's avatar
Thomas Jund committed
214 215 216 217 218 219 220 221 222 223 224
	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
225 226
}

227 228 229
void EnvMap::initGL()
{
#ifdef EXPORTING3
230 231
	std::vector<std::string> filenames;
	std::vector<std::string> texturenames;
232

233 234 235 236
	std::string dir("./meshRessources/cityTex/");
	DIR *dp;
	struct dirent *dirp;
	if((dp  = opendir(dir.c_str())) == NULL)
237
	{
238 239
		cout << "Error(" << errno << ") opening " << dir << endl;
		return;
240 241
	}

242 243 244 245 246 247 248
	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;
		}
249

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

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

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

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


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

268
	for(unsigned int i = 0 ; i < filenames.size() ; ++i)
269
	{
270
		PFP::MAP * nmap = new PFP::MAP();
271

272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290
		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());
291

292 293 294
		Utils::VBO * texcoordVBO = new Utils::VBO();
		Utils::VBO * positionVBO = new Utils::VBO();
		Utils::VBO * normalVBO = new Utils::VBO();
295

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

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

303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346
		texture->setWrapping(GL_CLAMP_TO_EDGE);

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

		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;

//	unsigned int i =0;
//	for(std::vector<PFP::MAP * >::iterator it = m_map_Export.begin() ; it != m_map_Export.end() ; ++it, ++i)
//	{
//
//		CityGenerator::planetify<PFP>(**it, position_nmap[i], 2000.0f, bbTest);
//	}
347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 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 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440

//	m_map_Export = new PFP2::MAP();
//
//	std::vector<std::string> attrNames ;
//	m_obj_Export = new Algo::Surface::Import::OBJModel<PFP2>(m_map_Export);
//	m_obj_Export->import("./meshRessources/cityTex/Building11.obj",attrNames);
//
//	position_Export = m_map_Export.getAttribute<VEC3, VERTEX>(attrNames[0]) ;
//	TraversorV<PFP2::MAP> tV(m_map_Export);
//
//	VEC3 bary(0);
//	unsigned int count = 0;
//	for(Dart d = tV.begin() ; d != tV.end() ; d = tV.next())
//	{
//		bary += position_Export[d];
//		count++;
//	}
//	bary /= float(count);
//
//	for(Dart d = tV.begin() ; d != tV.end() ; d = tV.next())
//	{
////		position_Export[d] -= bary;
//		position_Export[d] *= 100.0f;
//		std::cout << position_Export[d] << std::endl;
//		position_Export[d] += VEC3(2000,0,0);
//		std::cout << position_Export[d] << std::endl;
//	}
//
//	m_texcoordVBO_Export = new Utils::VBO();
//	m_positionVBO_Export = new Utils::VBO();
//	m_normalVBO_Export = new Utils::VBO();
//
//	m_texture_Export = new Utils::Texture<2,Geom::Vec3uc>(GL_UNSIGNED_BYTE);
//
//	if (m_texture_Export->load("./meshRessources/cityTex/AO_Buildings11.png"))
//		m_texture_Export->update();
//	else
//		std::cout << "problem : loading texture" << std::endl;
//
//	m_texture_Export->setWrapping(GL_CLAMP_TO_EDGE);
//
//	m_shaderTex_Export = new ShaderCustomTex();
//	m_shaderTex_Export->setAttributePosition(m_positionVBO_Export);
//	m_shaderTex_Export->setAttributeTexCoord(m_texcoordVBO_Export);
//	m_shaderTex_Export->setTextureUnit(GL_TEXTURE0);
//	m_shaderTex_Export->setTexture(m_texture_Export);
//
//	glEnable(GL_TEXTURE_2D);
//
//	m_map_Export.setBrowser(NULL);
//
//	if (!m_obj_Export->hasNormals())
//	{
//		normal_Export  = m_map_Export.getAttribute<VEC3, VERTEX>("normal") ;
//		if(!normal_Export.isValid())
//			normal_Export = m_map_Export.addAttribute<VEC3, VERTEX>("normal") ;
//
//		Algo::Surface::Geometry::computeNormalVertices<PFP2>(m_map_Export, m_obj_Export->m_positions, normal_Export) ;
//		m_obj_Export->setNormalAttribute(normal_Export);
//	}
//
//	m_nbIndice_Export = m_obj_Export->createSimpleVBO_PTN(m_positionVBO_Export,m_texcoordVBO_Export,m_normalVBO_Export);
#endif
}

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

	for(unsigned int i = 0 ; i < m_obj_Export.size() ; ++i)
	{
		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_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
}

441 442 443 444 445 446 447 448 449 450 451 452
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;

}

453 454
void EnvMap::markPedWay()
{
David Cazier's avatar
David Cazier committed
455
	CellMarker<FACE> treat(map) ;
David Cazier's avatar
David Cazier committed
456 457 458 459
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!treat.isMarked(d))
		{
460 461 462
			treat.mark(d) ;

			Dart dd = d ;
David Cazier's avatar
David Cazier committed
463 464
			do
			{
465
				Dart ddd = dd ;
David Cazier's avatar
David Cazier committed
466 467 468 469
				do
				{
					if (obstacleMark.isMarked(dd))
					{
470 471
						pedWayMark.mark(d) ;
						break ;
472
					}
473 474
					ddd = map.alpha1(ddd) ;
				} while (ddd != dd) ;
475

476
				dd = map.phi1(dd) ;
477

478
			} while (dd != d) ;
479 480 481
		}
	}

482
	treat.unmarkAll() ;
David Cazier's avatar
David Cazier committed
483 484 485 486
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!treat.isMarked(d))
		{
487 488 489
			treat.mark(d) ;

			Dart dd = d ;
David Cazier's avatar
David Cazier committed
490 491 492 493 494 495
			do
			{
				if (pedWayMark.isMarked(map.phi2(dd)) && pedWayMark.isMarked(map.phi2(map.phi1(dd)))
				    && !pedWayMark.isMarked(map.phi2(map.phi1(map.phi1(dd))))
				    && !pedWayMark.isMarked(map.phi2(map.phi1(map.phi1(map.phi1(dd))))))
				{
496 497 498
					pedWayMark.mark(d) ;
					break ;
				}
499

500
				dd = map.phi1(dd) ;
501

502
			} while (dd != d) ;
503 504 505 506
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
507 508
unsigned int EnvMap::mapMemoryCost()
{
David Cazier's avatar
David Cazier committed
509 510 511 512
	return (map.getAttributeContainer<DART>()).memorySize()
	    + (map.getAttributeContainer<VERTEX>()).memorySize()
	    + (map.getAttributeContainer<EDGE>()).memorySize()
	    + (map.getAttributeContainer<FACE>()).memorySize() ;
Thomas's avatar
Thomas committed
513 514
}

Pierre Kraemer's avatar
Pierre Kraemer committed
515
#ifndef SPATIAL_HASHING
Thomas's avatar
Thomas committed
516 517
void EnvMap::subdivideAllToMaxLevel()
{
518
	bool subdiv ;
David Cazier's avatar
David Cazier committed
519 520
	do
	{
521
		subdiv = false ;
Thomas's avatar
Thomas committed
522
		{
David Cazier's avatar
David Cazier committed
523
			CellMarker<FACE> subd(map) ;
David Cazier's avatar
David Cazier committed
524 525 526 527
			for (Dart d = map.begin(); d != map.end(); map.next(d))
			{
				if (!subd.isMarked(d))
				{
528
					subd.mark(d) ;
David Cazier's avatar
David Cazier committed
529 530
					if (!buildingMark.isMarked(d))
					{
Thomas's avatar
Thomas committed
531 532
						//check if subdivision is authorized
						float minDistSq = Agent::neighborDistSq_ ;
533
						bool subdivisable = true ;
Thomas's avatar
Thomas committed
534 535 536
						Dart old = map.faceOldestDart(d) ;
						unsigned int fLevel = map.faceLevel(old) ;
						map.setCurrentLevel(fLevel) ;
537
						PFP::VEC3 fCenter = Algo::Surface::Geometry::faceCentroid<PFP>(map, old, position) ;
Thomas's avatar
Thomas committed
538
						Dart fd = old ;
David Cazier's avatar
David Cazier committed
539 540
						do
						{
Thomas's avatar
Thomas committed
541
							PFP::VEC3& p = position[fd] ;
542
							PFP::VEC3 edge = Algo::Surface::Geometry::vectorOutOfDart<PFP>(map, fd,
David Cazier's avatar
David Cazier committed
543
							                                                      position) ;
544
							PFP::VEC3 proj = fCenter
David Cazier's avatar
David Cazier committed
545 546 547
							    - (p + (edge * (fCenter - p) / edge.norm2()) * edge) ;
							if (proj.norm2() < minDistSq)
							{
Thomas's avatar
Thomas committed
548 549 550 551
								subdivisable = false ;
								break ;
							}
							fd = map.phi1(fd) ;
552
						} while (fd != old) ;
Thomas's avatar
Thomas committed
553

David Cazier's avatar
David Cazier committed
554 555
						if (subdivisable)
						{
Thomas's avatar
Thomas committed
556
							map.setCurrentLevel(fLevel) ;
557
							Algo::Surface::IHM::subdivideFace<PFP>(map, old, position) ;
558
							subdiv = true ;
Thomas's avatar
Thomas committed
559 560 561 562 563 564
						}
						map.setCurrentLevel(map.getMaxLevel()) ;
					}
				}
			}
		}
565
	} while (subdiv) ;
Thomas's avatar
Thomas committed
566 567 568 569
}

void EnvMap::subdivideToProperLevel()
{
570
	bool subdiv ;
David Cazier's avatar
David Cazier committed
571 572
	do
	{
573
		subdiv = false ;
Thomas's avatar
Thomas committed
574
		{
David Cazier's avatar
David Cazier committed
575
			CellMarker<FACE> subd(map) ;
David Cazier's avatar
David Cazier committed
576 577 578 579
			for (Dart d = map.begin(); d != map.end(); map.next(d))
			{
				if (!subd.isMarked(d))
				{
580
					subd.mark(d) ;
David Cazier's avatar
David Cazier committed
581 582
					if (!refineMark.isMarked(d) && agentvect[d].size() > nbAgentsToSubdivide)
					{
583
						std::pair<bool, bool>& sf = subdivisableFace[d] ;
David Cazier's avatar
David Cazier committed
584 585
						if (sf.first == false || (sf.first == true && sf.second))
						{
586 587 588
							subdiv = true ;
							refineMark.mark(d) ;
							refineCandidate.push_back(d) ;
Thomas's avatar
Thomas committed
589 590 591 592
						}
					}
				}
			}
593
			subd.unmarkAll() ;
Thomas's avatar
Thomas committed
594
		}
595 596 597 598
		updateMap() ;
		refineCandidate.clear() ;
		map.setCurrentLevel(map.getMaxLevel()) ;
	} while (subdiv) ;
Thomas's avatar
Thomas committed
599 600
}

Pierre Kraemer's avatar
Pierre Kraemer committed
601 602
Dart EnvMap::getBelongingCell(const PFP::VEC3& pos)
{
pitiot's avatar
...  
pitiot committed
603
//	assert(map.getCurrentLevel() == map.getMaxLevel()) ;
pitiot's avatar
pitiot committed
604

David Cazier's avatar
David Cazier committed
605
	CellMarkerStore<FACE> m(map) ;
David Cazier's avatar
David Cazier committed
606 607 608 609
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!m.isMarked(d))
		{
610
			m.mark(d) ;
pitiot's avatar
pitiot committed
611
			if (!buildingMark.isMarked(d)
612
			    && Algo::Surface::Geometry::isPointInConvexFace2D<PFP>(map, d, position, pos, true)) return d ;
613
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
614 615
	}

616
	std::cout << "ERROR : pos not in map for getBelongingCell " << pos << std::endl ;
617
	return map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
618
}
Pierre Kraemer's avatar
Pierre Kraemer committed
619
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
620

Pierre Kraemer's avatar
Pierre Kraemer committed
621
#ifndef SPATIAL_HASHING
Pierre Kraemer's avatar
Pierre Kraemer committed
622 623
void EnvMap::foreach_neighborFace(Dart d, FunctorType& f)
{
624
	Dart dd = d ;
David Cazier's avatar
David Cazier committed
625 626
	do
	{
627
		Dart ddd = map.alpha1(map.alpha1(dd)) ;
David Cazier's avatar
David Cazier committed
628 629
		while (ddd != dd)
		{
630 631
			f(ddd) ;
			ddd = map.alpha1(ddd) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
632
		}
633 634
		dd = map.phi1(dd) ;
	} while (dd != d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
635 636
}

David Cazier's avatar
David Cazier committed
637 638 639 640 641 642 643 644 645 646 647 648 649 650 651 652 653
// 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
			{
654
				if (obstacleMark.isMarked(dd) && map.isBoundaryMarked2(dd))
David Cazier's avatar
David Cazier committed
655 656 657 658 659 660 661 662 663 664 665 666 667 668 669 670 671 672 673 674 675 676 677 678 679 680 681 682
				{
					Dart dd2 = map.phi2(dd) ;
					Dart next = map.phi1(dd2) ;
					Dart previous = map.phi_1(dd2) ;
					Obstacle* o = new Obstacle(position[next], position[dd2], position[previous],
					                           position[map.phi1(next)], NULL, 0) ;
					obstvect[dd2].push_back(o) ;
				}
				dd = map.phi1(dd) ;
			} while (dd != d) ;

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

}

Pierre Kraemer's avatar
Pierre Kraemer committed
683 684
void EnvMap::registerObstaclesInFaces()
{
David Cazier's avatar
David Cazier committed
685
	CellMarker<FACE> m(map) ;
David Cazier's avatar
David Cazier committed
686 687 688 689
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!m.isMarked(d))
		{
690
			m.mark(d) ;
David Cazier's avatar
David Cazier committed
691 692
			if (!buildingMark.isMarked(d))
			{
693
				Dart dd = d ;
694 695 696 697

				//retrieve all obstacles within its one-ring

				//first : test all edges of the face of d
David Cazier's avatar
David Cazier committed
698 699 700 701
				do
				{
					if (obstacleMark.isMarked(dd))
					{
702 703 704 705
						Dart dd2 = map.phi2(dd) ;
						Dart next = map.phi1(dd2) ;
						Dart previous = map.phi_1(dd2) ;
						Obstacle* o = new Obstacle(position[dd2], position[next],
David Cazier's avatar
David Cazier committed
706 707
						                           position[previous], position[map.phi1(next)],
						                           NULL, 0) ;
708
						obstvect[d].push_back(o) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
709
					}
710 711
					dd = map.phi1(dd) ;
				} while (dd != d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
712

713
				//second : test all edges of neighboring faces
David Cazier's avatar
David Cazier committed
714 715
				do
				{
716
					Dart ddd = map.alpha1(map.alpha1(dd)) ;
David Cazier's avatar
David Cazier committed
717 718
					while (ddd != dd)
					{
David Cazier's avatar
David Cazier committed
719
						// TODO Check effet de bord
David Cazier's avatar
David Cazier committed
720 721
						if (!buildingMark.isMarked(ddd)) addNeighborObstacles(
						    obstvect[d], ddd, ddd == map.alpha_1(dd)) ;
722
						ddd = map.alpha1(ddd) ;
723
					}
724 725
					dd = map.phi1(dd) ;
				} while (dd != d) ;
726 727 728
			}
		}
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
729 730 731 732
}

void EnvMap::addNeighborObstacles(PFP::OBSTACLES& obst, Dart d, bool edgeNeighbor)
{
733
	Dart stop ;
David Cazier's avatar
David Cazier committed
734 735 736 737
	if (edgeNeighbor)
		stop = map.phi_1(map.phi_1(map.phi_1(d))) ;
	else
		stop = map.phi_1(map.phi_1(d)) ;
738 739

	Dart dd = d ;
David Cazier's avatar
David Cazier committed
740 741 742 743
	do
	{
		if (obstacleMark.isMarked(dd))
		{
744 745 746 747 748 749 750 751 752 753
//			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
//			{
754 755 756 757
			Dart dd2 = map.phi2(dd) ;
			Dart next = map.phi1(dd2) ;
			Dart previous = map.phi_1(dd2) ;
			Obstacle* o = new Obstacle(position[dd2], position[next], position[previous],
David Cazier's avatar
David Cazier committed
758
			                           position[map.phi1(next)], NULL, 0) ;
759
			obst.push_back(o) ;
760
//			}
Pierre Kraemer's avatar
Pierre Kraemer committed
761
		}
762 763
		dd = map.phi1(dd) ;
	} while (dd != stop) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
764 765
}

766 767 768 769 770 771 772 773 774 775
void EnvMap::pushObstacleInCells(Obstacle * o)// réenregistre l'obstacle en question
{
	MovingObstacle * mo = o->mo;
	if (mo != NULL)
	{
		int n = o->index;

		Dart d1=NIL;
		Dart d2=NIL;
		std::vector<Dart> memo;
776
		memo = mo->getMemoCross(o->p1,o->p2,d1);
777 778 779 780 781 782 783 784 785 786 787 788 789
		d2=mo->registering_part->d;

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

Thomas Jund's avatar
Thomas Jund committed
790
Dart EnvMap::popAndPushObstacleInCells(Obstacle* o, int n)// maj de l'enregistrement
791 792 793 794 795 796 797 798 799 800 801 802
{
	MovingObstacle * mo = o->mo;

	Dart d1=NIL;
	Dart d2=NIL;
	std::vector<Dart> memo;

//	bool modif=false;

//	if(p1->crossCell != CGoGN::Algo::MovingObjects::NO_CROSS
//		|| p2->crossCell != CGoGN::Algo::MovingObjects::NO_CROSS)
//	{
803
		memo = mo->getMemoCross(o->p1,o->p2,d1);
804 805 806 807 808 809 810 811 812 813 814 815 816 817 818 819 820 821 822 823 824 825 826 827 828
		d2=mo->registering_part->d;
//		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(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
829 830

	return d1;
831 832 833 834 835 836 837 838 839 840 841 842 843 844 845 846 847 848 849 850 851 852 853 854 855 856 857 858 859 860 861 862 863 864 865 866 867 868 869 870 871 872 873 874 875 876 877 878 879 880 881 882
}

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->neighbor_cells[n].clear();

	Dart dd = d;
	do
	{
		Dart ddd = map.alpha1(map.alpha1(dd));
		while(ddd != dd)
		{
			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->belonging_cells[n] = memo_cross;

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

	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;
Jund Thomas's avatar
Jund Thomas committed
883 884
		addElementToVector<Obstacle*>(neighborObstvect[*it],o);
//		pushObstNeighborInCells(o, *it);
885 886 887 888 889 890 891 892 893 894 895 896 897 898 899 900 901 902 903 904 905 906 907
	}
}

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

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

	if (mo != NULL)
	{
		for (std::vector<Dart>::iterator it = mo->belonging_cells[n].begin();	it != mo->belonging_cells[n].end(); ++it)
		{
			removeElementFromVector<Obstacle*>(obstvect[*it], o) ;
		}

		for (std::vector<Dart>::iterator it = mo->neighbor_cells[n].begin(); it != mo->neighbor_cells[n].end(); ++it)
		{
			removeElementFromVector<Obstacle*>(neighborObstvect[*it], o) ;
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
908 909
void EnvMap::agentChangeFace(Agent* agent, Dart oldFace)
{
910
	Dart newFace = agent->part_.d ;
911

912 913
	popAgentInCells(agent, oldFace) ;
	pushAgentInCells(agent, newFace) ;
914

David Cazier's avatar
David Cazier committed
915 916
	if (!refineMark.isMarked(newFace) && agentvect[newFace].size() > nbAgentsToSubdivide)
	{
917
		std::pair<bool, bool>& sf = subdivisableFace[newFace] ;
David Cazier's avatar
David Cazier committed
918 919
		if (sf.first == false || (sf.first == true && sf.second))
		{
920 921
			refineMark.mark(newFace) ;
			refineCandidate.push_back(newFace) ;
922 923
		}
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
924

David Cazier's avatar
David Cazier committed
925 926 927
	if (!coarsenMark.isMarked(oldFace) && !refineMark.isMarked(oldFace)
	    && 4 * agentvect[oldFace].size() < nbAgentsToSimplify)
	{
928 929
		coarsenMark.mark(oldFace) ;
		coarsenCandidate.push_back(map.faceOldestDart(oldFace)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
930
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
931 932
}

933
void EnvMap::refine()
Pierre Kraemer's avatar
Pierre Kraemer committed
934
{
935
	for (std::vector<Dart>::iterator it = refineCandidate.begin(); it != refineCandidate.end(); ++it)
David Cazier's avatar
David Cazier committed
936 937
	{
		Dart d = (*it) ;
938
		refineMark.unmark(d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
939

940
		//if the number of agents is big enough
David Cazier's avatar
David Cazier committed
941 942
		if (agentvect[d].size() > nbAgentsToSubdivide)
		{
Thomas's avatar
Thomas committed
943
			int fLevel = -1 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
944 945
			Dart old = map.faceOldestDart(d) ;

946
			bool subdivisable = true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
947

948
			//check if faces resulting from a subdivision are big enough
949
			std::pair<bool, bool>& sf = subdivisableFace[old] ;
David Cazier's avatar
David Cazier committed
950 951 952 953
			if (sf.first == true)
				subdivisable = sf.second ;
			else
			{
954 955
//				float minSizeSq = minCellSize * minCellSize ; // diametre de vision de l'agent au carré
				float minSizeSq = Agent::neighborDistSq_;
956

Pierre Kraemer's avatar
Pierre Kraemer committed
957
				fLevel = map.faceLevel(old) ;
958
				map.setCurrentLevel(fLevel) ;
959
				PFP::VEC3 fCenter = Algo::Surface::Geometry::faceCentroid<PFP>(map, old, position) ;
960
				Dart fd = old ;
David Cazier's avatar
David Cazier committed
961 962
				do
				{
Pierre Kraemer's avatar
Pierre Kraemer committed
963
					PFP::VEC3& p = position[fd] ;
964
					PFP::VEC3 edge = Algo::Surface::Geometry::vectorOutOfDart<PFP>(map, fd, position) ;
David Cazier's avatar
David Cazier committed
965 966 967
					PFP::VEC3 proj = fCenter - (p + (edge * (fCenter - p) / edge.norm2()) * edge) ;
					if (proj.norm2() < minSizeSq)
					{
968 969
						subdivisable = false ;
						break ;
Pierre Kraemer's avatar
Pierre Kraemer committed
970
					}
971
					fd = map.phi1(fd) ;
972
				} while (fd != old) ;
973 974
				map.setCurrentLevel(map.getMaxLevel()) ;

Pierre Kraemer's avatar
Pierre Kraemer committed
975 976
				sf.first = true ;
				sf.second = subdivisable ;
977 978
			}

David Cazier's avatar
David Cazier committed
979 980
			if (subdivisable)
			{
981 982
				if (fLevel == -1)
					fLevel = map.faceLevel(old) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
983 984

				sf.first = false ;
985

986
				PFP::AGENTS oldAgents(agentvect[old]) ;
David Cazier's avatar
David Cazier committed
987 988
				PFP::OBSTACLEVECT oldObst(obstvect[old]) ;
				PFP::OBSTACLEVECT oldNeighborObst(neighborObstvect[old]) ;
Thomas Jund's avatar
Thomas Jund committed
989

David Cazier's avatar
David Cazier committed
990
				for (PFP::AGENTS::iterator ait = oldAgents.begin(); ait != oldAgents.end(); ++ait)
Thomas Jund's avatar
Thomas Jund committed
991
				{
David Cazier's avatar
David Cazier committed
992
					popAgentInCells(*ait, old) ;
Thomas Jund's avatar
Thomas Jund committed
993
				}
David Cazier's avatar
David Cazier committed
994
				for (PFP::OBSTACLEVECT::iterator ait = oldObst.begin(); ait != oldObst.end(); ++ait)
995 996 997
					this->popObstacleInCells(*ait, (*ait)->index) ;
				for (PFP::OBSTACLEVECT::iterator ait = oldNeighborObst.begin();ait != oldNeighborObst.end(); ++ait)
					this->popObstacleInCells(*ait, (*ait)->index) ;
998
				neighborAgentvect[old].clear() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
999 1000

				map.setCurrentLevel(fLevel) ;
1001
				Algo::Surface::IHM::subdivideFace<PFP>(map, old, position) ;
David Cazier's avatar
David Cazier committed
1002
				CellMarkerStore<FACE> newF(map) ;
1003 1004
				unsigned int degree = 0 ;
				Dart dd = old ;
David Cazier's avatar
David Cazier committed
1005 1006
				do
				{
1007
					++degree ;
1008
					newF.mark(dd) ;
1009
					dd = map.phi1(dd) ;
1010
				} while (dd != old) ;
David Cazier's avatar
David Cazier committed
1011

David Cazier's avatar
David Cazier committed
1012 1013
				if (degree == 3)
				{
1014
					Dart centerFace = map.phi2(map.phi1(old)) ;
1015
					newF.mark(centerFace) ;
1016
				}
Pierre Kraemer's avatar
Pierre Kraemer committed
1017
				map.setCurrentLevel(map.getMaxLevel()) ;
1018

1019
				//agents contained in the subdivided cell are pushed correctly
David Cazier's avatar
David Cazier committed
1020 1021 1022 1023
				for (PFP::AGENTS::iterator ait = oldAgents.begin(); ait != oldAgents.end(); ++ait)
				{
					resetAgentInFace(*ait) ;
					pushAgentInCells(*ait, (*ait)->part_.d) ;
1024
				}
Pierre Kraemer's avatar
Pierre Kraemer committed
1025

1026
				//same for obstacles contained
David Cazier's avatar
David Cazier committed
1027 1028
				for (PFP::OBSTACLEVECT::iterator ait = oldObst.begin(); ait != oldObst.end(); ++ait)
				{
1029
					resetPartSubdiv(*ait);