env_map.cpp 29.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

16 17
#include "env_generator.h"

18
using namespace CGoGN ;
Pierre Kraemer's avatar
Pierre Kraemer committed
19

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

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

Jund Thomas's avatar
Jund Thomas committed
40
	positionScenary = mapScenary.addAttribute<VEC3, VERTEX>("position") ;
Jund Thomas's avatar
Jund Thomas committed
41
	normalScenary = mapScenary.addAttribute<VEC3, VERTEX>("normal") ;
Jund Thomas's avatar
Jund Thomas committed
42

Pierre Kraemer's avatar
Pierre Kraemer committed
43
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
44 45 46 47 48
	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
49

50 51 52 53 54 55 56 57
	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
58
	VEC3 bottomLeft(-width / 2, -height / 2, 0.0f) ;
59 60 61 62 63 64 65 66
	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
67
	std::cout << " - Geometry size : " << geometry.size(0)<<" x "<<geometry.size(1) ;
68 69
	std::cout << " - Cell size between : " << minSize << " and " << maxSize << std::endl ;
#ifdef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
70
	std::cout << " - Table de hachage : " << agentGridSize(0) << " x " << agentGridSize(1) << std::endl ;
71 72
#endif

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

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

Jund Thomas's avatar
Jund Thomas committed
102 103
			Geom::BoundingBox<typename PFP::VEC3> bb1, bb2 ;
			bb1 = Algo::Geometry::computeBoundingBox<PFP>(map, position) ;
Jund Thomas's avatar
Jund Thomas committed
104
			bb2 = Algo::Geometry::computeBoundingBox<PFP>(mapScenary, positionScenary) ;
Jund Thomas's avatar
Jund Thomas committed
105 106 107
			bb1.addPoint(bb2.min());
			bb1.addPoint(bb2.max());

Jund Thomas's avatar
Jund Thomas committed
108 109
			CityGenerator::planetify<PFP>(map, position, 100.0f, bb1);
			CityGenerator::planetify<PFP>(mapScenary, positionScenary, 100.0f, bb1);
Jund Thomas's avatar
Jund Thomas committed
110 111

			std::vector<Dart> v;
Jund Thomas's avatar
Jund Thomas committed
112 113 114
			TraversorF<PFP::MAP> tF(mapScenary);
			for(Dart d = tF.begin() ; d != tF.end() ; d = tF.next())
			{
Jund Thomas's avatar
Jund Thomas committed
115 116 117 118 119 120 121 122 123
				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
124 125 126 127 128 129 130 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 169 170 171 172 173 174 175 176
				//test if convex
				bool convex=true;

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

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

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

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

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

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

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

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

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

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

Jund Thomas's avatar
Jund Thomas committed
177 178 179
			Algo::Modelisation::EarTriangulation<PFP> et2(map);
			et2.triangule();

Jund Thomas's avatar
Jund Thomas committed
180
			//subdivision to create footpath
Jund Thomas's avatar
Jund Thomas committed
181 182 183
//			SelectorDartNoBoundary<PFP::MAP> nb(map);
//			Algo::Modelisation::TwoNPlusOneSubdivision<PFP,PFP::TVEC3, VEC3>(map,position,nb);
//			markPedWay();
David Cazier's avatar
David Cazier committed
184
		}
Jund Thomas's avatar
Jund Thomas committed
185
		break;
186 187 188 189 190 191 192 193 194
	}

//	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
195 196
	// TODO Check registerWallInFaces();
	registerWallInFaces() ;
197 198
//	subdivideAllToMaxLevel();

199
	for (unsigned int i = subdivisableFace.begin(); i < subdivisableFace.end(); subdivisableFace.next(i))
200
		subdivisableFace[i].first = false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
201
#endif
202

Pierre Kraemer's avatar
Pierre Kraemer committed
203 204
}

205 206
void EnvMap::markPedWay()
{
David Cazier's avatar
David Cazier committed
207
	CellMarker<FACE> treat(map) ;
David Cazier's avatar
David Cazier committed
208 209 210 211
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!treat.isMarked(d))
		{
212 213 214
			treat.mark(d) ;

			Dart dd = d ;
David Cazier's avatar
David Cazier committed
215 216
			do
			{
217
				Dart ddd = dd ;
David Cazier's avatar
David Cazier committed
218 219 220 221
				do
				{
					if (obstacleMark.isMarked(dd))
					{
222 223
						pedWayMark.mark(d) ;
						break ;
224
					}
225 226
					ddd = map.alpha1(ddd) ;
				} while (ddd != dd) ;
227

228
				dd = map.phi1(dd) ;
229

230
			} while (dd != d) ;
231 232 233
		}
	}

234
	treat.unmarkAll() ;
David Cazier's avatar
David Cazier committed
235 236 237 238
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!treat.isMarked(d))
		{
239 240 241
			treat.mark(d) ;

			Dart dd = d ;
David Cazier's avatar
David Cazier committed
242 243 244 245 246 247
			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))))))
				{
248 249 250
					pedWayMark.mark(d) ;
					break ;
				}
251

252
				dd = map.phi1(dd) ;
253

254
			} while (dd != d) ;
255 256 257 258
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
259 260
unsigned int EnvMap::mapMemoryCost()
{
David Cazier's avatar
David Cazier committed
261 262 263 264
	return (map.getAttributeContainer<DART>()).memorySize()
	    + (map.getAttributeContainer<VERTEX>()).memorySize()
	    + (map.getAttributeContainer<EDGE>()).memorySize()
	    + (map.getAttributeContainer<FACE>()).memorySize() ;
Thomas's avatar
Thomas committed
265 266
}

Pierre Kraemer's avatar
Pierre Kraemer committed
267
#ifndef SPATIAL_HASHING
Thomas's avatar
Thomas committed
268 269
void EnvMap::subdivideAllToMaxLevel()
{
270
	bool subdiv ;
David Cazier's avatar
David Cazier committed
271 272
	do
	{
273
		subdiv = false ;
Thomas's avatar
Thomas committed
274
		{
David Cazier's avatar
David Cazier committed
275
			CellMarker<FACE> subd(map) ;
David Cazier's avatar
David Cazier committed
276 277 278 279
			for (Dart d = map.begin(); d != map.end(); map.next(d))
			{
				if (!subd.isMarked(d))
				{
280
					subd.mark(d) ;
David Cazier's avatar
David Cazier committed
281 282
					if (!buildingMark.isMarked(d))
					{
Thomas's avatar
Thomas committed
283 284
						//check if subdivision is authorized
						float minDistSq = Agent::neighborDistSq_ ;
285
						bool subdivisable = true ;
Thomas's avatar
Thomas committed
286 287 288
						Dart old = map.faceOldestDart(d) ;
						unsigned int fLevel = map.faceLevel(old) ;
						map.setCurrentLevel(fLevel) ;
David Cazier's avatar
David Cazier committed
289
						PFP::VEC3 fCenter = Algo::Geometry::faceCentroid<PFP>(map, old, position) ;
Thomas's avatar
Thomas committed
290
						Dart fd = old ;
David Cazier's avatar
David Cazier committed
291 292
						do
						{
Thomas's avatar
Thomas committed
293
							PFP::VEC3& p = position[fd] ;
294
							PFP::VEC3 edge = Algo::Geometry::vectorOutOfDart<PFP>(map, fd,
David Cazier's avatar
David Cazier committed
295
							                                                      position) ;
296
							PFP::VEC3 proj = fCenter
David Cazier's avatar
David Cazier committed
297 298 299
							    - (p + (edge * (fCenter - p) / edge.norm2()) * edge) ;
							if (proj.norm2() < minDistSq)
							{
Thomas's avatar
Thomas committed
300 301 302 303
								subdivisable = false ;
								break ;
							}
							fd = map.phi1(fd) ;
304
						} while (fd != old) ;
Thomas's avatar
Thomas committed
305

David Cazier's avatar
David Cazier committed
306 307
						if (subdivisable)
						{
Thomas's avatar
Thomas committed
308 309
							map.setCurrentLevel(fLevel) ;
							Algo::IHM::subdivideFace<PFP>(map, old, position) ;
310
							subdiv = true ;
Thomas's avatar
Thomas committed
311 312 313 314 315 316
						}
						map.setCurrentLevel(map.getMaxLevel()) ;
					}
				}
			}
		}
317
	} while (subdiv) ;
Thomas's avatar
Thomas committed
318 319 320 321
}

void EnvMap::subdivideToProperLevel()
{
322
	bool subdiv ;
David Cazier's avatar
David Cazier committed
323 324
	do
	{
325
		subdiv = false ;
Thomas's avatar
Thomas committed
326
		{
David Cazier's avatar
David Cazier committed
327
			CellMarker<FACE> subd(map) ;
David Cazier's avatar
David Cazier committed
328 329 330 331
			for (Dart d = map.begin(); d != map.end(); map.next(d))
			{
				if (!subd.isMarked(d))
				{
332
					subd.mark(d) ;
David Cazier's avatar
David Cazier committed
333 334
					if (!refineMark.isMarked(d) && agentvect[d].size() > nbAgentsToSubdivide)
					{
335
						std::pair<bool, bool>& sf = subdivisableFace[d] ;
David Cazier's avatar
David Cazier committed
336 337
						if (sf.first == false || (sf.first == true && sf.second))
						{
338 339 340
							subdiv = true ;
							refineMark.mark(d) ;
							refineCandidate.push_back(d) ;
Thomas's avatar
Thomas committed
341 342 343 344
						}
					}
				}
			}
345
			subd.unmarkAll() ;
Thomas's avatar
Thomas committed
346
		}
347 348 349 350
		updateMap() ;
		refineCandidate.clear() ;
		map.setCurrentLevel(map.getMaxLevel()) ;
	} while (subdiv) ;
Thomas's avatar
Thomas committed
351 352
}

Pierre Kraemer's avatar
Pierre Kraemer committed
353 354
Dart EnvMap::getBelongingCell(const PFP::VEC3& pos)
{
pitiot's avatar
...  
pitiot committed
355
//	assert(map.getCurrentLevel() == map.getMaxLevel()) ;
pitiot's avatar
pitiot committed
356

David Cazier's avatar
David Cazier committed
357
	CellMarkerStore<FACE> m(map) ;
David Cazier's avatar
David Cazier committed
358 359 360 361
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!m.isMarked(d))
		{
362
			m.mark(d) ;
pitiot's avatar
pitiot committed
363
			if (!buildingMark.isMarked(d)
David Cazier's avatar
David Cazier committed
364
			    && Algo::Geometry::isPointInConvexFace2D<PFP>(map, d, position, pos, true)) return d ;
365
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
366 367
	}

pitiot's avatar
pitiot committed
368
	std::cout << "ERROR : pos not in map for getBelongingCell" << std::endl ;
369
	return map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
370
}
Pierre Kraemer's avatar
Pierre Kraemer committed
371
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
372

Pierre Kraemer's avatar
Pierre Kraemer committed
373
#ifndef SPATIAL_HASHING
Pierre Kraemer's avatar
Pierre Kraemer committed
374 375
void EnvMap::foreach_neighborFace(Dart d, FunctorType& f)
{
376
	Dart dd = d ;
David Cazier's avatar
David Cazier committed
377 378
	do
	{
379
		Dart ddd = map.alpha1(map.alpha1(dd)) ;
David Cazier's avatar
David Cazier committed
380 381
		while (ddd != dd)
		{
382 383
			f(ddd) ;
			ddd = map.alpha1(ddd) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
384
		}
385 386
		dd = map.phi1(dd) ;
	} while (dd != d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
387 388
}

David Cazier's avatar
David Cazier committed
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
// TODO Check void EnvMap::registerWallInFaces() boundary markers ?
void EnvMap::registerWallInFaces()
{
	CellMarker<FACE> m(map) ;
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!m.isMarked(d))
		{
			m.mark(d) ;

			Dart dd = d ;

			//retrieve all obstacles within its one-ring

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

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

}

Pierre Kraemer's avatar
Pierre Kraemer committed
435 436
void EnvMap::registerObstaclesInFaces()
{
David Cazier's avatar
David Cazier committed
437
	CellMarker<FACE> m(map) ;
David Cazier's avatar
David Cazier committed
438 439 440 441
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!m.isMarked(d))
		{
442
			m.mark(d) ;
David Cazier's avatar
David Cazier committed
443 444
			if (!buildingMark.isMarked(d))
			{
445
				Dart dd = d ;
446 447 448 449

				//retrieve all obstacles within its one-ring

				//first : test all edges of the face of d
David Cazier's avatar
David Cazier committed
450 451 452 453
				do
				{
					if (obstacleMark.isMarked(dd))
					{
454 455 456 457
						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
458 459
						                           position[previous], position[map.phi1(next)],
						                           NULL, 0) ;
460
						obstvect[d].push_back(o) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
461
					}
462 463
					dd = map.phi1(dd) ;
				} while (dd != d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
464

465
				//second : test all edges of neighboring faces
David Cazier's avatar
David Cazier committed
466 467
				do
				{
468
					Dart ddd = map.alpha1(map.alpha1(dd)) ;
David Cazier's avatar
David Cazier committed
469 470
					while (ddd != dd)
					{
David Cazier's avatar
David Cazier committed
471
						// TODO Check effet de bord
David Cazier's avatar
David Cazier committed
472 473
						if (!buildingMark.isMarked(ddd)) addNeighborObstacles(
						    obstvect[d], ddd, ddd == map.alpha_1(dd)) ;
474
						ddd = map.alpha1(ddd) ;
475
					}
476 477
					dd = map.phi1(dd) ;
				} while (dd != d) ;
478 479 480
			}
		}
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
481 482 483 484
}

void EnvMap::addNeighborObstacles(PFP::OBSTACLES& obst, Dart d, bool edgeNeighbor)
{
485
	Dart stop ;
David Cazier's avatar
David Cazier committed
486 487 488 489
	if (edgeNeighbor)
		stop = map.phi_1(map.phi_1(map.phi_1(d))) ;
	else
		stop = map.phi_1(map.phi_1(d)) ;
490 491

	Dart dd = d ;
David Cazier's avatar
David Cazier committed
492 493 494 495
	do
	{
		if (obstacleMark.isMarked(dd))
		{
496 497 498 499 500 501 502 503 504 505
//			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
//			{
506 507 508 509
			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
510
			                           position[map.phi1(next)], NULL, 0) ;
511
			obst.push_back(o) ;
512
//			}
Pierre Kraemer's avatar
Pierre Kraemer committed
513
		}
514 515
		dd = map.phi1(dd) ;
	} while (dd != stop) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
516 517
}

518 519 520 521 522 523 524 525 526 527 528 529 530 531 532 533 534 535 536 537 538 539 540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569 570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632 633 634 635 636
void EnvMap::pushObstacleInCells(Obstacle * o)// réenregistre l'obstacle en question
{
	MovingObstacle * mo = o->mo;
	if (mo != NULL)
	{
		int n = o->index;
		VEC3 p1 = mo->vertices[n];
		VEC3 p2 = mo->vertices[(n+1)%(mo->nbVertices)];

		Dart d1=NIL;
		Dart d2=NIL;
		std::vector<Dart> memo;
		memo = mo->getMemoCross(p1,p2,d1);
		d2=mo->registering_part->d;

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

void EnvMap::popAndPushObstacleInCells(Obstacle* o, int n)// maj de l'enregistrement
{
	MovingObstacle * mo = o->mo;
	VEC3 p1 = mo->vertices[n];
	VEC3 p2 = mo->vertices[(n+1)%mo->nbVertices];

	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)
//	{
		memo = mo->getMemoCross(p1,p2,d1);
		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);
		}
	}
}

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
637 638
		addElementToVector<Obstacle*>(neighborObstvect[*it],o);
//		pushObstNeighborInCells(o, *it);
639 640 641 642 643 644 645 646 647 648 649 650 651 652 653 654 655 656 657 658 659 660 661
	}
}

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
662 663
void EnvMap::agentChangeFace(Agent* agent, Dart oldFace)
{
664
	Dart newFace = agent->part_.d ;
665

666 667
	popAgentInCells(agent, oldFace) ;
	pushAgentInCells(agent, newFace) ;
668

David Cazier's avatar
David Cazier committed
669 670
	if (!refineMark.isMarked(newFace) && agentvect[newFace].size() > nbAgentsToSubdivide)
	{
671
		std::pair<bool, bool>& sf = subdivisableFace[newFace] ;
David Cazier's avatar
David Cazier committed
672 673
		if (sf.first == false || (sf.first == true && sf.second))
		{
674 675
			refineMark.mark(newFace) ;
			refineCandidate.push_back(newFace) ;
676 677
		}
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
678

David Cazier's avatar
David Cazier committed
679 680 681
	if (!coarsenMark.isMarked(oldFace) && !refineMark.isMarked(oldFace)
	    && 4 * agentvect[oldFace].size() < nbAgentsToSimplify)
	{
682 683
		coarsenMark.mark(oldFace) ;
		coarsenCandidate.push_back(map.faceOldestDart(oldFace)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
684
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
685 686
}

687
void EnvMap::refine()
Pierre Kraemer's avatar
Pierre Kraemer committed
688
{
689
	for (std::vector<Dart>::iterator it = refineCandidate.begin(); it != refineCandidate.end(); ++it)
David Cazier's avatar
David Cazier committed
690 691
	{
		Dart d = (*it) ;
692
		refineMark.unmark(d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
693

694
		//if the number of agents is big enough
David Cazier's avatar
David Cazier committed
695 696
		if (agentvect[d].size() > nbAgentsToSubdivide)
		{
Thomas's avatar
Thomas committed
697
			int fLevel = -1 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
698 699
			Dart old = map.faceOldestDart(d) ;

700
			bool subdivisable = true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
701

702
			//check if faces resulting from a subdivision are big enough
703
			std::pair<bool, bool>& sf = subdivisableFace[old] ;
David Cazier's avatar
David Cazier committed
704 705 706 707
			if (sf.first == true)
				subdivisable = sf.second ;
			else
			{
708
				float minSizeSq = minCellSize * minCellSize ; // diametre de vision de l'agent au carré
709

Pierre Kraemer's avatar
Pierre Kraemer committed
710
				fLevel = map.faceLevel(old) ;
711 712 713
				map.setCurrentLevel(fLevel) ;
				PFP::VEC3 fCenter = Algo::Geometry::faceCentroid<PFP>(map, old, position) ;
				Dart fd = old ;
David Cazier's avatar
David Cazier committed
714 715
				do
				{
Pierre Kraemer's avatar
Pierre Kraemer committed
716
					PFP::VEC3& p = position[fd] ;
717
					PFP::VEC3 edge = Algo::Geometry::vectorOutOfDart<PFP>(map, fd, position) ;
David Cazier's avatar
David Cazier committed
718 719 720
					PFP::VEC3 proj = fCenter - (p + (edge * (fCenter - p) / edge.norm2()) * edge) ;
					if (proj.norm2() < minSizeSq)
					{
721 722
						subdivisable = false ;
						break ;
Pierre Kraemer's avatar
Pierre Kraemer committed
723
					}
724
					fd = map.phi1(fd) ;
725
				} while (fd != old) ;
726 727
				map.setCurrentLevel(map.getMaxLevel()) ;

Pierre Kraemer's avatar
Pierre Kraemer committed
728 729
				sf.first = true ;
				sf.second = subdivisable ;
730 731
			}

David Cazier's avatar
David Cazier committed
732 733
			if (subdivisable)
			{
734 735
				if (fLevel == -1)
					fLevel = map.faceLevel(old) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
736 737

				sf.first = false ;
738

739
				PFP::AGENTS oldAgents(agentvect[old]) ;
David Cazier's avatar
David Cazier committed
740 741
				PFP::OBSTACLEVECT oldObst(obstvect[old]) ;
				PFP::OBSTACLEVECT oldNeighborObst(neighborObstvect[old]) ;
David Cazier's avatar
David Cazier committed
742 743
				for (PFP::AGENTS::iterator ait = oldAgents.begin(); ait != oldAgents.end(); ++ait)
					popAgentInCells(*ait, old) ;
David Cazier's avatar
David Cazier committed
744
				for (PFP::OBSTACLEVECT::iterator ait = oldObst.begin(); ait != oldObst.end(); ++ait)
745 746 747
					this->popObstacleInCells(*ait, (*ait)->index) ;
				for (PFP::OBSTACLEVECT::iterator ait = oldNeighborObst.begin();ait != oldNeighborObst.end(); ++ait)
					this->popObstacleInCells(*ait, (*ait)->index) ;
748
				neighborAgentvect[old].clear() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
749 750

				map.setCurrentLevel(fLevel) ;
751
				Algo::IHM::subdivideFace<PFP>(map, old, position) ;
David Cazier's avatar
David Cazier committed
752
				CellMarkerStore<FACE> newF(map) ;
753 754
				unsigned int degree = 0 ;
				Dart dd = old ;
David Cazier's avatar
David Cazier committed
755 756
				do
				{
757
					++degree ;
758
					newF.mark(dd) ;
759
					dd = map.phi1(dd) ;
760
				} while (dd != old) ;
David Cazier's avatar
David Cazier committed
761

David Cazier's avatar
David Cazier committed
762 763
				if (degree == 3)
				{
764
					Dart centerFace = map.phi2(map.phi1(old)) ;
765
					newF.mark(centerFace) ;
766
				}
Pierre Kraemer's avatar
Pierre Kraemer committed
767
				map.setCurrentLevel(map.getMaxLevel()) ;
768

769
				//agents contained in the subdivided cell are pushed correctly
David Cazier's avatar
David Cazier committed
770 771 772 773
				for (PFP::AGENTS::iterator ait = oldAgents.begin(); ait != oldAgents.end(); ++ait)
				{
					resetAgentInFace(*ait) ;
					pushAgentInCells(*ait, (*ait)->part_.d) ;
774
				}
Pierre Kraemer's avatar
Pierre Kraemer committed
775

776
				//same for obstacles contained
David Cazier's avatar
David Cazier committed
777 778
				for (PFP::OBSTACLEVECT::iterator ait = oldObst.begin(); ait != oldObst.end(); ++ait)
				{
779
					resetPartSubdiv(*ait);
780
					pushObstacleInCells(*ait);
pitiot's avatar
maj  
pitiot committed
781

David Cazier's avatar
David Cazier committed
782 783
				}

784
				//same for adjacent obstacles
David Cazier's avatar
David Cazier committed
785
				for (PFP::OBSTACLEVECT::iterator ait = oldNeighborObst.begin();
786
					ait != oldNeighborObst.end(); ++ait)
David Cazier's avatar
David Cazier committed
787
				{
788
					pushObstacleInCells(*ait) ;
David Cazier's avatar
David Cazier committed
789 790
				}

791
				//retrieve neighbors agents from onering cells
792
				dd = old ;
David Cazier's avatar
David Cazier committed
793 794
				do
				{
Pierre Kraemer's avatar
Pierre Kraemer committed
795
					Dart d3 = dd ;
David Cazier's avatar
David Cazier committed
796 797
					do
					{
Pierre Kraemer's avatar
Pierre Kraemer committed
798 799
						Dart d4 = map.alpha1(map.alpha1(d3)) ;
						PFP::AGENTS& nad3 = neighborAgentvect[d3] ;
David Cazier's avatar
David Cazier committed
800 801 802 803
						while (d4 != d3)
						{
							if (!newF.isMarked(d4))
							{
Pierre Kraemer's avatar
Pierre Kraemer committed
804 805 806 807
								PFP::AGENTS& ad4 = agentvect[d4] ;
								nad3.insert(nad3.end(), ad4.begin(), ad4.end()) ;
							}
							d4 = map.alpha1(d4) ;
808
						}
Pierre Kraemer's avatar
Pierre Kraemer committed
809
						d3 = map.phi1(d3) ;
810
					} while (d3 != dd) ;
811 812

					map.setCurrentLevel(fLevel) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
813
					dd = map.phi1(dd) ;
814
					map.setCurrentLevel(map.getMaxLevel()) ;
815
				} while (dd != old) ;
816

David Cazier's avatar
David Cazier committed
817 818
				if (degree == 3)
				{
Pierre Kraemer's avatar
Pierre Kraemer committed
819 820
					Dart centerFace = map.phi2(map.phi1(old)) ;
					Dart d3 = centerFace ;
David Cazier's avatar
David Cazier committed
821 822
					do
					{
Pierre Kraemer's avatar
Pierre Kraemer committed
823 824
						Dart d4 = map.alpha1(map.alpha1(d3)) ;
						PFP::AGENTS& nad3 = neighborAgentvect[d3] ;
David Cazier's avatar
David Cazier committed
825 826 827 828
						while (d4 != d3)
						{
							if (!newF.isMarked(d4))
							{
Pierre Kraemer's avatar
Pierre Kraemer committed
829
								PFP::AGENTS& ad4 = agentvect[d4] ;
David Cazier's avatar
David Cazier committed
830
								nad3.insert(neighborAgentvect[d3].end(), ad4.begin(), ad4.end()) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
831 832
							}
							d4 = map.alpha1(d4) ;
833
						}
Pierre Kraemer's avatar
Pierre Kraemer committed
834
						d3 = map.phi1(d3) ;
835
					} while (d3 != centerFace) ;
836
				}
Pierre Kraemer's avatar
Pierre Kraemer committed
837 838 839
			}
		}
	}
David Cazier's avatar
David Cazier committed
840 841
	refineCandidate.clear() ;

842
}
843 844


845 846
void EnvMap::coarse()
{
David Cazier's avatar
David Cazier committed
847 848
	// On recrée une liste des faces à simplifier en empêchant les doublons
	// On en profite pour vérifier les conditions de simplifications
849 850
	std::vector<Dart> checkCoarsenCandidate ;
	checkCoarsenCandidate.reserve(coarsenCandidate.size()) ;
851

852 853 854 855 856 857 858
	for (unsigned int it = 0; it < coarsenCandidate.size(); ++it)
	{
		Dart old = coarsenCandidate[it] ;
		bool oldIsMarked = coarsenMark.isMarked(old) ;
		coarsenMark.unmark(old) ;

		unsigned int fLevel = map.faceLevel(old) ;
859

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

865
			if (map.faceIsSubdividedOnce(old))
866
			{
867 868 869 870 871 872 873 874 875 876 877 878 879 880
				// on compte le nombre d'agents dans les sous-faces
				// on en profite pour compter le degré de la face grossière
				unsigned int degree = 0 ;
				unsigned int nbAgents = 0 ;
				Dart fit = old ;
				do
				{
					nbAgents += agentvect[fit].size() ;
					++degree ;
					coarsenMark.unmark(fit) ;
// TODO Optimisation déjà faite
//					unsigned int start = it + 1;
//					unsigned int fEmb = map.getEmbedding<FACE>(fit) ;
//					while(start < coarsenCandidate.size())
David Cazier's avatar
David Cazier committed
881
//					{
882 883 884 885 886 887 888
//						if(map.getEmbedding<FACE>(coarsenCandidate[start]) == fEmb)
//						{
//							coarsenCandidate[start] = coarsenCandidate.back() ;
//							coarsenCandidate.pop_back() ;
//						}
//						else
//							++start ;
David Cazier's avatar
David Cazier committed
889
//					}
890 891 892 893

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

894

895 896
				//Loop subdivision
				if (degree == 3)
897
				{
898 899 900 901 902 903 904 905 906 907 908 909 910 911 912 913 914 915
					map.setCurrentLevel(fLevel) ;
					Dart centerFace = map.phi2(map.phi1(old)) ;
					nbAgents += agentvect[centerFace].size() ;
					coarsenMark.unmark(centerFace) ;
// TODO Optimisation déjà faite
//					unsigned int start = it + 1;
//					unsigned int fEmb = map.getEmbedding<FACE>(centerFace) ;
//					while(start < coarsenCandidate.size())
//					{
//						if(map.getEmbedding<FACE>(coarsenCandidate[start]) == fEmb)
//						{
//							coarsenCandidate[start] = coarsenCandidate.back() ;
//							coarsenCandidate.pop_back() ;
//						}
//						else
//							++start ;
//					}
					map.setCurrentLevel(fLevel - 1) ;
916
				}
917
				if (nbAgents < nbAgentsToSimplify) checkCoarsenCandidate.push_back(old) ;
918
			}
919
			map.setCurrentLevel(cur) ;
920
		}
921 922
	}
	coarsenCandidate.clear() ;
923

924
//		 On réalise la simplification (les conditions ont déjà été vérifiées)
925 926 927 928 929 930 931 932 933 934 935 936 937 938 939 940 941 942 943 944 945 946 947
	for (unsigned int it = 0; it < checkCoarsenCandidate.size(); ++it)
	{
		Dart old = checkCoarsenCandidate[it] ;

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

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

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

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

954 955
			map.setCurrentLevel(map.getMaxLevel()) ;
			for(PFP::OBSTACLEVECT::iterator ait = ob.begin(); ait != ob.end(); ++ait)
956
			{
957 958 959
				this->popObstacleInCells(*ait, (*ait)->index) ;
				obst.push_back(*ait);
			}
960 961


962

963 964 965 966 967 968 969 970 971 972
			for (PFP::AGENTS::iterator ait = a.begin(); ait != a.end(); ++ait)
				popAgentInCells(*ait, fit) ;
			map.setCurrentLevel(fLevel - 1) ;
			Dart nf = map.phi2(fit) ;
			if (!map.faceIsSubdivided(nf))
			{
				map.setCurrentLevel(fLevel) ;
				PFP::AGENTS& an = agentvect[nf] ;
				PFP::OBSTACLEVECT resetob = obstvect[nf] ;
				for (PFP::AGENTS::iterator ait = an.begin(); ait != an.end(); ++ait)
973
				{
974
					if ((*ait)->part_.d == map.phi1(nf)) (*ait)->part_.d = nf ;
975
				}
976
				for (PFP::OBSTACLEVECT::iterator ait = resetob.begin(); ait != resetob.end(); ++ait)
977
				{
978 979
					MovingObstacle* mo = (*ait)->mo ;
					if (mo != NULL)
980
					{
981
						resetPart(mo,nf) ;
982
					}
pitiot's avatar