env_map.cpp 19.3 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"
Pierre Kraemer's avatar
Pierre Kraemer committed
5

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

13 14
#include "env_generator.h"

Pierre Kraemer's avatar
Pierre Kraemer committed
15 16
using namespace CGoGN;

Pierre Kraemer's avatar
Pierre Kraemer committed
17
EnvMap::EnvMap() :
18 19
	obstacleMarkS(mapScenary, EDGE),
	buildingMarkS(mapScenary, FACE),
Pierre Kraemer's avatar
Pierre Kraemer committed
20 21
	obstacleMark(map, EDGE),
	buildingMark(map, FACE),
Pierre Kraemer's avatar
Pierre Kraemer committed
22 23 24
	pedWayMark(map, FACE)
#ifndef SPATIAL_HASHING
	,
Pierre Kraemer's avatar
Pierre Kraemer committed
25 26
	refineMark(map, FACE),
	coarsenMark(map, FACE)
Pierre Kraemer's avatar
Pierre Kraemer committed
27 28 29 30 31 32 33 34 35 36 37 38 39 40 41
#else
	,
	origin(70 * 24 / 2.0, 70 * 24 / 2.0, 0.0),
	a_cell_size_x(Agent::neighborDist_),
	a_cell_size_y(Agent::neighborDist_),
	a_cell_nb_x(70 * 24 / a_cell_size_x),
	a_cell_nb_y(70 * 24 / a_cell_size_y),
	o_cell_size_x(Agent::range_),
	o_cell_size_y(Agent::range_),
	o_cell_nb_x(70 * 24 / o_cell_size_x),
	o_cell_nb_y(70 * 24 / o_cell_size_y),
	ht_agents(1024),
	ht_neighbor_agents(1024),
	ht_obstacles(1024)
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
42
{
Pierre Kraemer's avatar
Pierre Kraemer committed
43 44
	position = map.addAttribute<PFP::VEC3>(VERTEX, "position");
	normal = map.addAttribute<PFP::VEC3>(VERTEX, "normal");
Pierre Kraemer's avatar
Pierre Kraemer committed
45 46

#ifndef SPATIAL_HASHING
Pierre Kraemer's avatar
Pierre Kraemer committed
47 48 49 50
	agentvect = map.addAttribute<PFP::AGENTVECT>(FACE, "agents");
	neighborAgentvect = map.addAttribute<PFP::AGENTVECT>(FACE, "neighborAgents");
	obstvect = map.addAttribute<PFP::OBSTACLEVECT>(FACE, "obstacles");
	subdivisableFace = map.addAttribute<PFP::BOOLATTRIB>(FACE, "subdivisableFace");
Thomas's avatar
Thomas committed
51 52 53

	refineCandidate.reserve(100);
	coarsenCandidate.reserve(100);
Pierre Kraemer's avatar
Pierre Kraemer committed
54
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
55 56
}

57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109
void EnvMap::markPedWay()
{
	CellMarker treat(map,FACE);
	for(Dart d = map.begin();d != map.end(); map.next(d))
	{
		if(!treat.isMarked(d))
		{
			treat.mark(d);

			Dart dd = d;
			do
			{
				Dart ddd = dd;
				do
				{
					if(obstacleMark.isMarked(dd))
					{
						pedWayMark.mark(d);
						break;
					}
					ddd = map.alpha1(ddd);
				} while(ddd != dd);

				dd = map.phi1(dd);

			} while (dd != d);
		}
	}

	treat.unmarkAll();
	for(Dart d = map.begin();d != map.end(); map.next(d))
	{
		if(!treat.isMarked(d))
		{
			treat.mark(d);

			Dart dd = d;
			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))))))
					{
						pedWayMark.mark(d);
						break;
					}

				dd = map.phi1(dd);

			} while (dd != d);
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
110 111
unsigned int EnvMap::mapMemoryCost()
{
Pierre Kraemer's avatar
Pierre Kraemer committed
112 113 114 115
	return (map.getAttributeContainer(DART)).memorySize()
			+ (map.getAttributeContainer(VERTEX)).memorySize()
			+ (map.getAttributeContainer(EDGE)).memorySize()
			+ (map.getAttributeContainer(FACE)).memorySize();
Pierre Kraemer's avatar
Pierre Kraemer committed
116 117
}

Thomas's avatar
Thomas committed
118 119 120 121 122 123 124 125 126
void EnvMap::scale(float scaleVal)
{
	Geom::BoundingBox<PFP::VEC3> bb(position.begin());

	for(unsigned int v = position.begin() ; v != position.end() ; position.next(v))
	{
		bb.addPoint(position[v]);
	}

127 128 129 130 131
	for(unsigned int v = positionScenary.begin() ; v != positionScenary.end() ; positionScenary.next(v))
	{
		bb.addPoint(positionScenary[v]);
	}

Thomas's avatar
Thomas committed
132 133 134 135 136 137
	VEC3 center = bb.center();
	for(unsigned int v = position.begin() ; v != position.end() ; position.next(v))
	{
		position[v] -= center;
		position[v] *= scaleVal;
	}
138 139 140 141 142 143

	for(unsigned int v = positionScenary.begin() ; v != positionScenary.end() ; positionScenary.next(v))
	{
		positionScenary[v] -= center;
		positionScenary[v] *= scaleVal;
	}
Thomas's avatar
Thomas committed
144 145
}

Pierre Kraemer's avatar
Pierre Kraemer committed
146
#ifndef SPATIAL_HASHING
Thomas's avatar
Thomas committed
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 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229
void EnvMap::subdivideAllToMaxLevel()
{
	bool subdiv;
	do
	{
		subdiv=false;
		{
			CellMarker subd(map,FACE);
			for(Dart d = map.begin(); d!= map.end(); map.next(d))
			{
				if(!subd.isMarked(d))
				{
					subd.mark(d);
					if(!buildingMark.isMarked(d))
					{
						//check if subdivision is authorized
						float minDistSq = Agent::neighborDistSq_ ;
						bool subdivisable = true;
						Dart old = map.faceOldestDart(d) ;
						unsigned int fLevel = map.faceLevel(old) ;
						map.setCurrentLevel(fLevel) ;
						PFP::VEC3 fCenter = Algo::Geometry::faceCentroid<PFP>(map, old, position) ;
						Dart fd = old ;
						do
						{
							PFP::VEC3& p = position[fd] ;
							PFP::VEC3 edge = Algo::Geometry::vectorOutOfDart<PFP>(map, fd, position) ;
							PFP::VEC3 proj = fCenter - (p + (edge * (fCenter - p) / edge.norm2()) * edge) ;
							if(proj.norm2() < minDistSq)
							{
								subdivisable = false ;
								break ;
							}
							fd = map.phi1(fd) ;
						} while(fd != old) ;

						if(subdivisable)
						{
							map.setCurrentLevel(fLevel) ;
							Algo::IHM::subdivideFace<PFP>(map, old, position) ;
							subdiv = true;
						}
						map.setCurrentLevel(map.getMaxLevel()) ;
					}
				}
			}
		}
	} while(subdiv);
}

void EnvMap::subdivideToProperLevel()
{
	bool subdiv;
	do
	{
		subdiv=false;
		{
			CellMarker subd(map,FACE);
			for(Dart d = map.begin(); d!= map.end(); map.next(d))
			{
				if(!subd.isMarked(d))
				{
					subd.mark(d);
					if(!refineMark.isMarked(d) && agentvect[d].size() > nbAgentsToSubdivide)
					{
						std::pair<bool,bool>& sf = subdivisableFace[d];
						if(sf.first == false || (sf.first == true && sf.second))
						{
							subdiv=true;
							refineMark.mark(d);
							refineCandidate.push_back(d);
						}
					}
				}
			}
			subd.unmarkAll();
		}
		updateMap();
		refineCandidate.clear();
		map.setCurrentLevel(map.getMaxLevel());
	} while(subdiv);
}

Pierre Kraemer's avatar
Pierre Kraemer committed
230 231
Dart EnvMap::getBelongingCell(const PFP::VEC3& pos)
{
Pierre Kraemer's avatar
Pierre Kraemer committed
232
	CellMarkerStore m(map, FACE);
Pierre Kraemer's avatar
Pierre Kraemer committed
233 234
	for(Dart d = map.begin(); d != map.end(); map.next(d))
	{
Pierre Kraemer's avatar
Pierre Kraemer committed
235 236
		if(!m.isMarked(d))
		{
237 238 239 240
			m.mark(d);
			if(!buildingMark.isMarked(d) && Algo::Geometry::isPointInConvexFace2D<PFP>(map, d, position, pos, true))
				return d;
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
241 242 243 244 245
	}

	std::cout << "ERROR : pos not in map" << std::endl;
	return map.begin();
}
Pierre Kraemer's avatar
Pierre Kraemer committed
246
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
247

Thomas's avatar
Thomas committed
248
void EnvMap::init(unsigned int config)
Pierre Kraemer's avatar
Pierre Kraemer committed
249 250
{
	float sideSize = 70.0f;
Pierre Kraemer's avatar
update  
Pierre Kraemer committed
251
	unsigned int nbSquares = 24;
Thomas's avatar
Thomas committed
252

Thomas's avatar
Thomas committed
253 254 255 256
	switch(config)
	{
	case 0 : CityGenerator::generateGrid<PFP>(map, position, nbSquares, nbSquares, sideSize, obstacleMark, buildingMark);
			break;
Thomas's avatar
Thomas committed
257
	case 1 : CityGenerator::generateCity<PFP>(this, sideSize, nbSquares);
Thomas's avatar
Thomas committed
258
			break;
Pierre Kraemer's avatar
Pierre Kraemer committed
259 260
	case 2 : CityGenerator::generateCity<PFP>(this, sideSize, nbSquares);
			// CityGenerator::generateMall<PFP>(map, position, obstacleMark, buildingMark, sideSize);
Thomas's avatar
Thomas committed
261 262 263 264 265 266 267 268
			break;
	case 3 :
	{
		//	std::string filename = "/home/jund/Desktop/drawingQuads.svg";
		//	std::string filename = "/home/jund/Desktop/drawingTest.svg";
		//	std::string filename = "/home/jund/Desktop/drawingSimple.svg";
		//	std::string filename = "/home/jund/Desktop/drawingLines.svg";
		//	std::string filename = "/home/jund/Desktop/mapKrutSimple.svg";
Pierre Kraemer's avatar
Pierre Kraemer committed
269 270 271 272 273 274 275 276
//			std::string filename = "/home/jund/Desktop/mapKrut.svg";
//			Algo::Import::importSVG<PFP>(map,filename,position, obstacleMark, buildingMark);
//			scale(3.2808399f);
//
//			Algo::BooleanOperator::mergeVertices<PFP>(map, position);
//			map.closeMap();
//			Algo::Modelisation::CatmullClarkSubdivision<PFP>(map, position);
//			Algo::Modelisation::computeDual<PFP>(map);
Thomas's avatar
Thomas committed
277 278 279 280 281
	}
			break;
	case 4 : CityGenerator::generatePlanet<PFP>(map, position, obstacleMark, buildingMark, 200.0f, nbSquares);
			break;
	}
282

Pierre Kraemer's avatar
Pierre Kraemer committed
283
//	CityGenerator::simplifyFreeSpace<PFP>(map, position, obstacleMark, buildingMark);
Thomas's avatar
Thomas committed
284 285
//	CityGenerator::convexifyFreeSpace<PFP>(map, position, obstacleMark, buildingMark);
//	CityGenerator::installGuardRail<PFP>(map, position, obstacleMark, buildingMark, 5.0f);
286

Pierre Kraemer's avatar
Pierre Kraemer committed
287
#ifndef SPATIAL_HASHING
Pierre Kraemer's avatar
Pierre Kraemer committed
288
	map.init();
289 290
//	registerObstaclesInFaces();

Pierre Kraemer's avatar
yeah..  
Pierre Kraemer committed
291
//	subdivideAllToMaxLevel();
Thomas's avatar
Thomas committed
292

293 294
	for(unsigned int i = subdivisableFace.begin(); i < subdivisableFace.end(); subdivisableFace.next(i))
		subdivisableFace[i].first = false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
295
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
296 297
}

Pierre Kraemer's avatar
Pierre Kraemer committed
298
#ifndef SPATIAL_HASHING
Pierre Kraemer's avatar
Pierre Kraemer committed
299 300 301 302 303 304 305 306 307 308 309 310 311 312 313
void EnvMap::foreach_neighborFace(Dart d, FunctorType& f)
{
	Dart dd = d;
	do
	{
		Dart ddd = map.alpha1(map.alpha1(dd));
		while(ddd != dd)
		{
			f(ddd);
			ddd = map.alpha1(ddd);
		}
		dd = map.phi1(dd);
	} while(dd != d);
}

Pierre Kraemer's avatar
Pierre Kraemer committed
314 315
void EnvMap::registerObstaclesInFaces()
{
Pierre Kraemer's avatar
Pierre Kraemer committed
316
	CellMarker m(map, FACE);
Pierre Kraemer's avatar
Pierre Kraemer committed
317 318 319 320 321
	for(Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if(!m.isMarked(d))
		{
			m.mark(d);
322
			if(!buildingMark.isMarked(d))
Pierre Kraemer's avatar
Pierre Kraemer committed
323
			{
324
				Dart dd = d;
325 326 327 328

				//retrieve all obstacles within its one-ring

				//first : test all edges of the face of d
329 330 331
				do
				{
					if(obstacleMark.isMarked(dd))
Pierre Kraemer's avatar
Pierre Kraemer committed
332 333 334 335 336 337 338
					{
						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], position[map.phi1(next)]);
						obstvect[d].push_back(o);
					}
339 340
					dd = map.phi1(dd);
				} while(dd != d);
Pierre Kraemer's avatar
Pierre Kraemer committed
341

342
				//second : test all edges of neighboring faces
343
				do
Pierre Kraemer's avatar
Pierre Kraemer committed
344
				{
345 346 347 348 349 350 351 352 353 354 355 356
					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
357 358 359 360 361 362 363 364 365 366 367 368 369 370 371
}

void EnvMap::addNeighborObstacles(PFP::OBSTACLES& obst, Dart d, bool edgeNeighbor)
{
	Dart stop;
	if(edgeNeighbor)
		stop = map.phi_1(map.phi_1(map.phi_1(d)));
	else
		stop = map.phi_1(map.phi_1(d));
	
	Dart dd = d;
	do
	{
		if(obstacleMark.isMarked(dd))
		{
372 373 374 375 376 377 378 379 380 381
//			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
//			{
Pierre Kraemer's avatar
Pierre Kraemer committed
382 383 384 385 386
				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], position[map.phi1(next)]);
				obst.push_back(o);
387
//			}
Pierre Kraemer's avatar
Pierre Kraemer committed
388 389
		}
		dd = map.phi1(dd);
Pierre Kraemer's avatar
Pierre Kraemer committed
390
	} while(dd != stop);
Pierre Kraemer's avatar
Pierre Kraemer committed
391 392
}

393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411
//void EnvMap::agentChangeFaceThroughEdge(Agent* agent)
//{
//	Dart oldFace = agent->part_.lastCrossed;
//	Dart newFace = agent->part_.d;
//
//	agentvect[newFace].push_back(agent);
//
//	PFP::AGENTS::iterator end = agentvect[oldFace].end();
//	for(PFP::AGENTS::iterator it = agentvect[oldFace].begin(); it != end; ++it)
//	{
//		if(*it == agent)
//		{
//			*it = agentvect[oldFace].back();
//			agentvect[oldFace].pop_back();
//			break;
//		}
//	}
//
//	// mark adjacent cells shared by newFace and oldFace
Pierre Kraemer's avatar
Pierre Kraemer committed
412
//	CellMarkerNoUnmark f(map, FACE);
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 441 442 443 444 445 446 447 448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469
//	Dart d = oldFace;
//	do
//	{
//		f.mark(d);
//		d = map.alpha1(d);
//	} while(d != oldFace);
//
//	d = map.phi1(oldFace);
//	do
//	{
//		f.mark(d);
//		d = map.alpha1(d);
//	} while(d != map.phi1(oldFace));
//
//	// remove agent from cells adjacent to oldFace but not to newFace
//	Dart dd = oldFace;
//	do
//	{
//		Dart ddd = map.alpha1(map.alpha1(dd));
//		while(ddd != dd)
//		{
//			if(!f.isMarked(ddd))
//			{
//				bool found = false;
//				PFP::AGENTS::iterator end = neighborAgentvect[ddd].end();
//				for(PFP::AGENTS::iterator it = neighborAgentvect[ddd].begin(); it != end && !found; ++it)
//				{
//					if(*it == agent)
//					{
//						*it = neighborAgentvect[ddd].back();
//						neighborAgentvect[ddd].pop_back();
//						found = true;
//					}
//				}
//			}
//			ddd = map.alpha1(ddd);
//		}
//		dd = map.phi1(dd);
//	} while(dd != oldFace);
//
//	// add agent from cells adjacent to oldFace but not to newFace
//	// and unmark shared cells
//	dd = newFace;
//	do
//	{
//		Dart ddd = map.alpha1(map.alpha1(dd));
//		while(ddd != dd)
//		{
//			if(!f.isMarked(ddd))
//				neighborAgentvect[ddd].push_back(agent);
//			else
//				f.unmark(ddd);
//			ddd = map.alpha1(ddd);
//		}
//		dd = map.phi1(dd);
//	} while(dd != newFace);
//}
Pierre Kraemer's avatar
Pierre Kraemer committed
470

Pierre Kraemer's avatar
Pierre Kraemer committed
471 472
void EnvMap::agentChangeFace(Agent* agent, Dart oldFace)
{
473 474
	Dart newFace = agent->part_.d;

Pierre Kraemer's avatar
Pierre Kraemer committed
475
	popAgentInCells(agent, oldFace);
476 477
	pushAgentInCells(agent, newFace);

Pierre Kraemer's avatar
Pierre Kraemer committed
478
	if(!refineMark.isMarked(newFace) && agentvect[newFace].size() > nbAgentsToSubdivide)
479 480 481 482 483 484 485 486
	{
		std::pair<bool,bool>& sf = subdivisableFace[newFace];
		if(sf.first == false || (sf.first == true && sf.second))
		{
			refineMark.mark(newFace);
			refineCandidate.push_back(newFace);
		}
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
487

Pierre Kraemer's avatar
Pierre Kraemer committed
488
	if(!coarsenMark.isMarked(oldFace) && !refineMark.isMarked(oldFace) && agentvect[oldFace].size() < nbAgentsToSimplify / 4)
Pierre Kraemer's avatar
Pierre Kraemer committed
489 490 491 492
	{
		coarsenMark.mark(oldFace);
		coarsenCandidate.push_back(map.faceOldestDart(oldFace));
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
493 494
}

Pierre Kraemer's avatar
Pierre Kraemer committed
495 496 497 498 499 500
void EnvMap::obstacleChangeFace(Obstacle* agent, Dart newFace, Dart oldFace)
{
	popObstacleInCells(agent, oldFace);
	pushObstacleInCells(agent, newFace);
}

Pierre Kraemer's avatar
Pierre Kraemer committed
501 502 503 504
void EnvMap::updateMap()
{
	assert(map.getCurrentLevel() == map.getMaxLevel()) ;

505
	for(std::vector<Dart>::iterator it = refineCandidate.begin(); it != refineCandidate.end(); ++it)
Pierre Kraemer's avatar
Pierre Kraemer committed
506
	{
507 508
		Dart d = (*it) ;
		refineMark.unmark(d);
Pierre Kraemer's avatar
Pierre Kraemer committed
509

Pierre Kraemer's avatar
Pierre Kraemer committed
510
		if(agentvect[d].size() > nbAgentsToSubdivide)
Pierre Kraemer's avatar
Pierre Kraemer committed
511
		{
Thomas's avatar
Thomas committed
512
			int fLevel = -1 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
513 514
			Dart old = map.faceOldestDart(d) ;

515
			bool subdivisable = true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
516

Pierre Kraemer's avatar
Pierre Kraemer committed
517
			std::pair<bool,bool>& sf = subdivisableFace[old] ;
Pierre Kraemer's avatar
Pierre Kraemer committed
518 519
			if(sf.first == true)
				subdivisable = sf.second ;
520
			else
Pierre Kraemer's avatar
Pierre Kraemer committed
521
			{
Pierre Kraemer's avatar
Pierre Kraemer committed
522
				float minDistSq = Agent::neighborDistSq_ ; // diametre de vision de l'agent au carré
523

Pierre Kraemer's avatar
Pierre Kraemer committed
524
				fLevel = map.faceLevel(old) ;
525 526 527
				map.setCurrentLevel(fLevel) ;
				PFP::VEC3 fCenter = Algo::Geometry::faceCentroid<PFP>(map, old, position) ;
				Dart fd = old ;
Pierre Kraemer's avatar
Pierre Kraemer committed
528 529
				do
				{
Pierre Kraemer's avatar
Pierre Kraemer committed
530
					PFP::VEC3& p = position[fd] ;
531
					PFP::VEC3 edge = Algo::Geometry::vectorOutOfDart<PFP>(map, fd, position) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
532
					PFP::VEC3 proj = fCenter - (p + (edge * (fCenter - p) / edge.norm2()) * edge) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
533
					if(proj.norm2() < minDistSq)
Pierre Kraemer's avatar
Pierre Kraemer committed
534
					{
535 536
						subdivisable = false ;
						break ;
Pierre Kraemer's avatar
Pierre Kraemer committed
537
					}
538 539 540 541
					fd = map.phi1(fd) ;
				} while(fd != old) ;
				map.setCurrentLevel(map.getMaxLevel()) ;

Pierre Kraemer's avatar
Pierre Kraemer committed
542 543
				sf.first = true ;
				sf.second = subdivisable ;
544 545 546 547
			}

			if(subdivisable)
			{
Pierre Kraemer's avatar
Pierre Kraemer committed
548
				if(fLevel == -1)
Pierre Kraemer's avatar
Pierre Kraemer committed
549
					fLevel = map.faceLevel(old) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
550 551

				sf.first = false ;
552 553 554 555 556 557

				PFP::AGENTS oldAgents(agentvect[old]);
				for(PFP::AGENTS::iterator ait = oldAgents.begin(); ait != oldAgents.end(); ++ait)
					popAgentInCells(*ait, old);

				neighborAgentvect[old].clear() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
558 559

				map.setCurrentLevel(fLevel) ;
560
				Algo::IHM::subdivideFace<PFP>(map, old, position) ;
561 562 563 564 565 566 567 568 569 570 571 572 573 574
				CellMarkerStore newF(map, FACE) ;
				unsigned int degree = 0 ;
				Dart dd = old ;
				do
				{
					++degree ;
					newF.mark(dd);
					dd = map.phi1(dd) ;
				} while(dd != old) ;
				if(degree == 3)
				{
					Dart centerFace = map.phi2(map.phi1(old)) ;
					newF.mark(centerFace);
				}
Pierre Kraemer's avatar
Pierre Kraemer committed
575
				map.setCurrentLevel(map.getMaxLevel()) ;
576

577 578 579 580 581
				for(PFP::AGENTS::iterator ait = oldAgents.begin(); ait != oldAgents.end(); ++ait)
				{
					resetAgentInFace(*ait) ;
					pushAgentInCells(*ait, (*ait)->part_.d) ;
				}
Pierre Kraemer's avatar
Pierre Kraemer committed
582

583
				dd = old ;
Pierre Kraemer's avatar
Pierre Kraemer committed
584 585
				do
				{
Pierre Kraemer's avatar
Pierre Kraemer committed
586
					Dart d3 = dd ;
587
					do
Pierre Kraemer's avatar
Pierre Kraemer committed
588
					{
Pierre Kraemer's avatar
Pierre Kraemer committed
589 590
						Dart d4 = map.alpha1(map.alpha1(d3)) ;
						PFP::AGENTS& nad3 = neighborAgentvect[d3] ;
591 592 593
						while(d4 != d3)
						{
							if(!newF.isMarked(d4))
Pierre Kraemer's avatar
Pierre Kraemer committed
594 595 596 597 598
							{
								PFP::AGENTS& ad4 = agentvect[d4] ;
								nad3.insert(nad3.end(), ad4.begin(), ad4.end()) ;
							}
							d4 = map.alpha1(d4) ;
599
						}
Pierre Kraemer's avatar
Pierre Kraemer committed
600 601
						d3 = map.phi1(d3) ;
					} while(d3 != dd) ;
602 603

					map.setCurrentLevel(fLevel) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
604
					dd = map.phi1(dd) ;
605 606 607 608 609
					map.setCurrentLevel(map.getMaxLevel()) ;
				} while(dd != old);

				if(degree == 3)
				{
Pierre Kraemer's avatar
Pierre Kraemer committed
610 611
					Dart centerFace = map.phi2(map.phi1(old)) ;
					Dart d3 = centerFace ;
612 613
					do
					{
Pierre Kraemer's avatar
Pierre Kraemer committed
614 615
						Dart d4 = map.alpha1(map.alpha1(d3)) ;
						PFP::AGENTS& nad3 = neighborAgentvect[d3] ;
616 617 618
						while(d4 != d3)
						{
							if(!newF.isMarked(d4))
Pierre Kraemer's avatar
Pierre Kraemer committed
619 620 621 622 623
							{
								PFP::AGENTS& ad4 = agentvect[d4] ;
								nad3.insert(neighborAgentvect[d3].end(), ad4.begin(), ad4.end()) ;
							}
							d4 = map.alpha1(d4) ;
624
						}
Pierre Kraemer's avatar
Pierre Kraemer committed
625 626
						d3 = map.phi1(d3) ;
					} while(d3 != centerFace) ;
627
				}
Pierre Kraemer's avatar
Pierre Kraemer committed
628 629 630
			}
		}
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
631

Thomas's avatar
Thomas committed
632
	for(unsigned int it = 0; it < coarsenCandidate.size(); ++it)
Pierre Kraemer's avatar
Pierre Kraemer committed
633
	{
Thomas's avatar
Thomas committed
634
		Dart old = coarsenCandidate[it] ;
Pierre Kraemer's avatar
Pierre Kraemer committed
635
		coarsenMark.unmark(old) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
636

Pierre Kraemer's avatar
Pierre Kraemer committed
637
		unsigned int fLevel = map.faceLevel(old) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
638 639 640 641 642 643 644 645

		if(fLevel > 0 && map.getDartLevel(old) < fLevel)
		{
			unsigned int cur = map.getCurrentLevel() ;
			map.setCurrentLevel(fLevel - 1) ;

			if(map.faceIsSubdividedOnce(old))
			{
Pierre Kraemer's avatar
Pierre Kraemer committed
646 647
				// on compte le nombre d'agents dans les sous-faces
				// on en profite pour compter le degré de la face grossière
Pierre Kraemer's avatar
Pierre Kraemer committed
648 649 650 651 652
				unsigned int degree = 0 ;
				unsigned int nbAgents = 0 ;
				Dart fit = old ;
				do
				{
Pierre Kraemer's avatar
Pierre Kraemer committed
653
					nbAgents += agentvect[fit].size() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
654
					++degree ;
Pierre Kraemer's avatar
Pierre Kraemer committed
655 656 657 658 659 660 661 662 663 664 665 666 667 668 669

					coarsenMark.unmark(fit) ;
					unsigned int start = it + 1;
					unsigned int fEmb = map.getEmbedding(FACE, fit) ;
					while(start < coarsenCandidate.size())
					{
						if(map.getEmbedding(FACE, coarsenCandidate[start]) == fEmb)
						{
							coarsenCandidate[start] = coarsenCandidate.back() ;
							coarsenCandidate.pop_back() ;
						}
						else
							++start ;
					}

Pierre Kraemer's avatar
Pierre Kraemer committed
670 671 672 673 674 675 676
					fit = map.phi1(fit) ;
				} while(fit != old) ;
				if(degree == 3)
				{
					map.setCurrentLevel(fLevel) ;
					Dart centerFace = map.phi2(map.phi1(old)) ;
					nbAgents += agentvect[centerFace].size() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
677 678 679 680 681 682 683 684 685 686 687 688 689
					coarsenMark.unmark(centerFace) ;
					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 ;
					}
Pierre Kraemer's avatar
Pierre Kraemer committed
690 691 692 693 694 695 696 697 698
					map.setCurrentLevel(fLevel - 1) ;
				}

				if(nbAgents < nbAgentsToSimplify)
				{
					PFP::AGENTS agents ;
					Dart fit = old ;
					do
					{
Pierre Kraemer's avatar
Pierre Kraemer committed
699 700 701
						PFP::AGENTS a(agentvect[fit]) ;
						agents.insert(agents.end(), a.begin(), a.end()) ;
						map.setCurrentLevel(map.getMaxLevel()) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
702
						for(PFP::AGENTS::iterator ait = a.begin(); ait != a.end(); ++ait)
Pierre Kraemer's avatar
Pierre Kraemer committed
703
							popAgentInCells(*ait, fit) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
704 705 706 707 708 709 710 711
						map.setCurrentLevel(fLevel - 1) ;
						Dart nf = map.phi2(fit) ;
						if(!map.faceIsSubdivided(nf))
						{
							map.setCurrentLevel(fLevel) ;
							PFP::AGENTS& an = agentvect[nf] ;
							for(PFP::AGENTS::iterator ait = an.begin(); ait != an.end(); ++ait)
							{
Pierre Kraemer's avatar
Pierre Kraemer committed
712 713
								if((*ait)->part_.d == map.phi1(nf))
									(*ait)->part_.d = nf ;
Pierre Kraemer's avatar
Pierre Kraemer committed
714 715 716 717 718 719 720 721 722
							}
							map.setCurrentLevel(fLevel - 1) ;
						}
						fit = map.phi1(fit) ;
					} while(fit != old) ;
					if(degree == 3)
					{
						map.setCurrentLevel(fLevel) ;
						Dart centerFace = map.phi2(map.phi1(old)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
723 724 725
						PFP::AGENTS a(agentvect[centerFace]) ;
						agents.insert(agents.end(), a.begin(), a.end()) ;
						map.setCurrentLevel(map.getMaxLevel());
Pierre Kraemer's avatar
Pierre Kraemer committed
726 727
						for(PFP::AGENTS::iterator ait = a.begin(); ait != a.end(); ++ait)
							popAgentInCells(*ait, centerFace);
Pierre Kraemer's avatar
Pierre Kraemer committed
728 729
						map.setCurrentLevel(fLevel - 1) ;
					}
Pierre Kraemer's avatar
Pierre Kraemer committed
730
					neighborAgentvect[old].clear();
Pierre Kraemer's avatar
Pierre Kraemer committed
731 732 733

					Algo::IHM::coarsenFace<PFP>(map, old, position) ;

Pierre Kraemer's avatar
Pierre Kraemer committed
734 735 736
					std::pair<bool,bool>& sf = subdivisableFace[old] ;
					sf.first = true;
					sf.second = true;
Pierre Kraemer's avatar
Pierre Kraemer committed
737 738 739

					map.setCurrentLevel(map.getMaxLevel());

Thomas's avatar
Thomas committed
740
					for(PFP::AGENTS::iterator itA = agents.begin(); itA != agents.end(); ++itA)
Pierre Kraemer's avatar
Pierre Kraemer committed
741
					{
Thomas's avatar
Thomas committed
742 743
						(*itA)->part_.d = old ;
						pushAgentInCells(*itA, old);
Pierre Kraemer's avatar
Pierre Kraemer committed
744
					}
Pierre Kraemer's avatar
Pierre Kraemer committed
745

Pierre Kraemer's avatar
Pierre Kraemer committed
746 747 748 749 750 751 752 753 754 755 756
					Dart dd = old;
					do
					{
						Dart ddd = map.alpha1(map.alpha1(dd));
						while(ddd != dd)
						{
							neighborAgentvect[old].insert(neighborAgentvect[old].end(), agentvect[ddd].begin(), agentvect[ddd].end());
							ddd = map.alpha1(ddd);
						}
						dd = map.phi1(dd);
					} while(dd != old);
Pierre Kraemer's avatar
Pierre Kraemer committed
757

Pierre Kraemer's avatar
Pierre Kraemer committed
758
					if(fLevel > 1 && !coarsenMark.isMarked(old) && agentvect[old].size() < nbAgentsToSimplify / 4)
Thomas's avatar
Thomas committed
759 760 761 762
					{
						coarsenMark.mark(old);
						coarsenCandidate.push_back(map.faceOldestDart(old));
					}
Pierre Kraemer's avatar
Pierre Kraemer committed
763 764 765 766 767
				}
			}
			map.setCurrentLevel(cur) ;
		}
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
768

Pierre Kraemer's avatar
Pierre Kraemer committed
769 770 771 772 773 774 775
	map.setCurrentLevel(map.getMaxLevel()) ;
}

void EnvMap::resetAgentInFace(Agent* agent)
{
	VEC3 pos = agent->part_.m_position;
	agent->part_.m_position = Algo::Geometry::faceCentroid<PFP>(map, agent->part_.d, position);
Pierre Kraemer's avatar
Pierre Kraemer committed
776
	agent->part_.state = FACE;
Pierre Kraemer's avatar
Pierre Kraemer committed
777 778
	agent->part_.move(pos);
}
Pierre Kraemer's avatar
Pierre Kraemer committed
779
#endif