env_map.h 11.6 KB
Newer Older
1 2
#ifndef ENV_MAP_H
#define ENV_MAP_H
Pierre Kraemer's avatar
Pierre Kraemer committed
3 4 5 6 7

#include <iostream>
#include <algorithm>

#include "Topology/generic/parameters.h"
Pierre Kraemer's avatar
update  
Pierre Kraemer committed
8
#include "Topology/map/embeddedMap2.h"
Pierre Kraemer's avatar
Pierre Kraemer committed
9 10 11 12 13 14 15 16 17

#include "Algo/ImplicitHierarchicalMesh/ihm.h"
#include "Algo/ImplicitHierarchicalMesh/subdivision.h"

#include "Algo/Modelisation/polyhedron.h"
#include "Algo/Modelisation/extrusion.h"
#include "Algo/Modelisation/subdivision.h"
#include "Algo/Geometry/centroid.h"
#include "Algo/Geometry/area.h"
Thomas's avatar
Thomas committed
18
#include "Geometry/bounding_box.h"
Pierre Kraemer's avatar
Pierre Kraemer committed
19 20 21 22 23

#include "Container/fakeAttribute.h"

#include "Algo/Parallel/parallel_foreach.h"

Pierre Kraemer's avatar
Pierre Kraemer committed
24 25
#include "spatialHashing.h"

26
using namespace CGoGN ;
Pierre Kraemer's avatar
Pierre Kraemer committed
27

pitiot's avatar
maj  
pitiot committed
28 29 30
class Agent;
class Obstacle;
class MovingObstacle;
Pierre Kraemer's avatar
Pierre Kraemer committed
31

David Cazier's avatar
David Cazier committed
32
struct PFP : public PFP_STANDARD
Pierre Kraemer's avatar
Pierre Kraemer committed
33 34
{
	// definition de la carte
David Cazier's avatar
David Cazier committed
35
	typedef Algo::IHM::ImplicitHierarchicalMap MAP ;
Pierre Kraemer's avatar
Pierre Kraemer committed
36 37

	// definition des listes d'agent
David Cazier's avatar
David Cazier committed
38 39
	typedef std::vector<Agent*> AGENTS ;
	typedef std::vector<Obstacle*> OBSTACLES ;
pitiot's avatar
pitiot committed
40

pitiot's avatar
maj  
pitiot committed
41 42
	typedef std::vector<MovingObstacle*> MOVINGOBSTACLES;

David Cazier's avatar
David Cazier committed
43 44
	typedef NoMathIONameAttribute<AGENTS> AGENTVECT ;
	typedef NoMathIONameAttribute<OBSTACLES> OBSTACLEVECT ;
pitiot's avatar
pitiot committed
45

pitiot's avatar
maj  
pitiot committed
46 47 48 49 50
	typedef NoMathIONameAttribute<MOVINGOBSTACLES> MOVINGOBSTACLEVECT;

	typedef VertexAttribute<PFP::VEC3> TVEC3;
	typedef FaceAttribute<AGENTVECT> TAB_AGENTVECT ;
	typedef FaceAttribute<OBSTACLEVECT> TAB_OBSTACLEVECT ;
51

David Cazier's avatar
David Cazier committed
52 53
	typedef NoMathIONameAttribute<std::pair<bool, bool> > BOOLATTRIB ;
} ;
Pierre Kraemer's avatar
Pierre Kraemer committed
54

David Cazier's avatar
David Cazier committed
55
typedef PFP::VEC3 VEC3 ;
56
typedef PFP::REAL REAL ;
Pierre Kraemer's avatar
Pierre Kraemer committed
57 58 59

class EnvMap
{
60 61
public:
	PFP::MAP map ;
62

63 64 65 66 67 68 69 70 71 72 73 74
	/* Geometry of the EnvMap : the bounding box of the scene
	 * The density of cells in the EnvMap is given by two parameters :
	 *  - minCellSize : the size under which no subdivision occurs
	 *  - maxCellSize : the size of the initial cells (before subdivisions occur)
	 *  - obstacleDistance : the minimal goal distance between agents and obstacles
	 * The number of cells in the initial EnvMap is about (width*height)/(size*size)
	 */
	Geom::BoundingBox<VEC3> geometry ;
	REAL maxCellSize ;
	REAL minCellSize ;
	REAL obstacleDistance ;

David Cazier's avatar
David Cazier committed
75
	EnvMap() ;
76
	void init(unsigned int config, REAL width, REAL height, REAL minSize, REAL maxSize) ;
77

David Cazier's avatar
David Cazier committed
78
	void markPedWay() ;
Thomas's avatar
Thomas committed
79

David Cazier's avatar
David Cazier committed
80
	unsigned int mapMemoryCost() ;
Thomas's avatar
Thomas committed
81

Pierre Kraemer's avatar
Pierre Kraemer committed
82
#ifndef SPATIAL_HASHING
83
	Dart getBelongingCell(const VEC3& pos) ;
Thomas's avatar
Thomas committed
84

85 86
	void subdivideAllToMaxLevel() ;
	void subdivideToProperLevel() ;
Thomas's avatar
Thomas committed
87

88
	void foreach_neighborFace(Dart d, FunctorType& f) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
89

90
	void registerObstaclesInFaces() ;
pitiot's avatar
maj  
pitiot committed
91
	void registerWallInFaces();
92
	void addNeighborObstacles(PFP::OBSTACLES& obst, Dart d, bool edgeNeighbor) ;
93

94
	void setAgentNeighborsAndObstacles(Agent* agent) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
95

96 97 98 99 100 101
	unsigned int totalNeighborSize(Dart d) ;
	void nbAgentsIncrease(Dart d) ;
	void nbAgentsDecrease(Dart d) ;
	void pushAgentInCells(Agent* agent, Dart d) ;
	void popAgentInCells(Agent* agent, Dart d) ;
	void agentChangeFace(Agent* agent, Dart oldFace) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
102

103 104 105 106 107 108
	void popAndPushObstacleInCells(Obstacle* o, int n);
	void pushObstacleInCells(Obstacle * mo);
	void pushObstacleInOneRingCells(Obstacle * o, Dart d, int n);
	void pushObstacleInCells(Obstacle* o, int n, const std::vector<Dart>& memo_cross);
	void popObstacleInCells(Obstacle* o, int n);

109
	void popObstacleInCells(Obstacle* o, Dart d) ;
110 111
	void refine() ;
	void coarse() ;
112
	void updateMap() ;
113

114
	void resetAgentInFace(Agent* agent) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
115 116
#endif

David Cazier's avatar
David Cazier committed
117 118
	PFP::TVEC3 position ;
	PFP::TVEC3 normal ;
Pierre Kraemer's avatar
Pierre Kraemer committed
119

David Cazier's avatar
David Cazier committed
120 121 122
	PFP::MAP mapScenary ;
	PFP::TVEC3 positionScenary ;
	PFP::TVEC3 normalScenary ;
pitiot's avatar
maj  
pitiot committed
123 124 125 126 127
	CellMarker<EDGE> obstacleMarkS ;
	CellMarker<FACE> buildingMarkS ;
	CellMarker<EDGE> obstacleMark ;
	CellMarker<FACE> buildingMark ;
	CellMarker<FACE> pedWayMark ;
128
//	CellMarkerMemo<FACE> memo_mark;
pitiot's avatar
maj  
pitiot committed
129
	// ajout moving obst
130 131
	void addObstAsNeighbor (Obstacle *  o, const std::vector<Dart>& belonging_cells, std::vector<Dart> * nieghbor_cells);
	// void addMovingObstAsNeighbor (MovingObstacle *  mo,std::vector<Dart> belonging_cells, std::vector<Dart> *neighbor_cells);
pitiot's avatar
maj  
pitiot committed
132 133
	void pushObstNeighborInCells(Obstacle* o, Dart d);
	void popObstNeighborInCells(Obstacle* o, Dart d);
134
	void find_next(Obstacle* o,Dart * d, CellMarkerMemo<FACE>& cms);
pitiot's avatar
maj  
pitiot committed
135

136

David Cazier's avatar
David Cazier committed
137
	std::vector<Dart> newBuildings ;
138

Pierre Kraemer's avatar
Pierre Kraemer committed
139
#ifndef SPATIAL_HASHING
pitiot's avatar
maj  
pitiot committed
140 141 142 143 144
	FaceAttribute<PFP::BOOLATTRIB> subdivisableFace ;

	PFP::TAB_AGENTVECT agentvect ;
	PFP::TAB_AGENTVECT neighborAgentvect ;
	PFP::TAB_OBSTACLEVECT neighborObstvect;
Pierre Kraemer's avatar
Pierre Kraemer committed
145
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
146

David Cazier's avatar
David Cazier committed
147
	PFP::TAB_OBSTACLEVECT obstvect ;
Pierre Kraemer's avatar
Pierre Kraemer committed
148 149


Pierre Kraemer's avatar
Pierre Kraemer committed
150
#ifndef SPATIAL_HASHING
151
	static const unsigned int nbAgentsToSubdivide = 5 ;
Jund Thomas's avatar
Jund Thomas committed
152
	static const unsigned int nbAgentsToSimplify = 4 ;
David Cazier's avatar
David Cazier committed
153

pitiot's avatar
maj  
pitiot committed
154 155
	CellMarker<FACE> refineMark ;
	CellMarker<FACE> coarsenMark ;
156 157
	std::vector<Dart> refineCandidate ;
	std::vector<Dart> coarsenCandidate ;
Pierre Kraemer's avatar
Pierre Kraemer committed
158

159
	void clearUpdateCandidates() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
160 161 162
#endif

#ifdef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
163 164 165 166 167 168 169 170 171
	unsigned int agentGridSize(unsigned int i)
	{
		return geometry.size(i) / minCellSize ;
	}

	unsigned int obstacleGridSize(unsigned int i)
	{
		return geometry.size(i) / obstacleDistance ;
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
172

pitiot's avatar
maj  
pitiot committed
173
	Geom::Vec2ui agentPositionCell(Agent* a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
174

pitiot's avatar
maj  
pitiot committed
175
	Geom::Vec2ui obstaclePositionCell(VEC3 pos)
Pierre Kraemer's avatar
Pierre Kraemer committed
176
	{
David Cazier's avatar
David Cazier committed
177 178
		VEC3 relativePos = pos - geometry.min() ;
		relativePos /= obstacleDistance ;
179
		return Geom::Vec2ui(relativePos[0], relativePos[1]) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
180 181
	}

pitiot's avatar
maj  
pitiot committed
182 183 184 185 186 187 188 189 190 191 192 193 194 195
	const std::vector<Agent*>& getNeighbors(Geom::Vec2ui c) {
		assert(ht_agents.hasData(c)) ;
		return ht_agents[c];
	}
	const std::vector<Agent*>& getNeighbors(Agent* a) ;

	void getOneRingNeighbors(Agent* a, std::vector<Agent*>& neighbors) ;

	void addAgentInGrid(Agent* a) ;
	void addAgentInGrid(Agent* a, Geom::Vec2ui c) ;

	void removeAgentFromGrid(Agent* a) ;
	void removeAgentFromGrid(Agent* a, Geom::Vec2ui c) ;

Pierre Kraemer's avatar
Pierre Kraemer committed
196 197 198
	HashTable2D< std::vector<Agent*> > ht_agents ;
	HashTable2D< std::vector<Obstacle*> > ht_obstacles ;
#endif
199
} ;
Pierre Kraemer's avatar
Pierre Kraemer committed
200

pitiot's avatar
maj  
pitiot committed
201 202
void update_registration(Obstacle * o);
void register_pop(Obstacle* o, int n);
203 204 205
void resetPartSubdiv(Obstacle* o);
void resetObstPartInFace(Obstacle* o, Dart d);// empeche de viser une dart ayant disparu
void resetPart(MovingObstacle * mo, Dart d); // empeche de viser une dart ayant disparu pour les voisins
pitiot's avatar
maj  
pitiot committed
206 207
void displayMO(Obstacle * o);

Pierre Kraemer's avatar
Pierre Kraemer committed
208
/**************************************
David Cazier's avatar
David Cazier committed
209 210
 *           INLINE FUNCTIONS          *
 **************************************/
Pierre Kraemer's avatar
Pierre Kraemer committed
211

212 213 214 215 216 217 218
template <typename T>
inline void addElementToVector(std::vector<T>& a, T ag)
{
	assert(std::find(a.begin(), a.end(), ag) == a.end());
	a.push_back(ag) ;
}

David Cazier's avatar
David Cazier committed
219
template <typename T>
pitiot's avatar
maj  
pitiot committed
220
inline bool removeElementFromVector(std::vector<T>& a, T ag)
Pierre Kraemer's avatar
Pierre Kraemer committed
221
{
222 223
	assert(std::find(a.begin(), a.end(), ag) != a.end());

224
	typename std::vector<T>::iterator end = a.template end() ;
David Cazier's avatar
David Cazier committed
225 226 227 228
	for (typename std::vector<T>::iterator it = a.begin(); it != end; ++it)
	{
		if (*it == ag)
		{
229 230
			*it = a.back() ;
			a.pop_back() ;
pitiot's avatar
maj  
pitiot committed
231
			return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
232 233
		}
	}
pitiot's avatar
maj  
pitiot committed
234
	return false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
235 236 237
}

#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
238 239
inline unsigned int EnvMap::totalNeighborSize(Dart d)
{
240 241 242 243
	return agentvect[d].size() + neighborAgentvect[d].size() ;
}

inline void EnvMap::nbAgentsIncrease(Dart d)
Pierre Kraemer's avatar
Pierre Kraemer committed
244
{
245 246
	if (refineMark.isMarked(d)) return ;
	if (totalNeighborSize(d) < nbAgentsToSubdivide) return ;
Pierre Kraemer's avatar
Pierre Kraemer committed
247

248
	std::pair<bool, bool>& sf = subdivisableFace[d] ;
David Cazier's avatar
David Cazier committed
249 250
	if (sf.first == false || (sf.first == true && sf.second))
	{
251 252 253
		refineMark.mark(d) ;
		refineCandidate.push_back(d) ;
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
254 255
}

256
inline void EnvMap::nbAgentsDecrease(Dart d)
Thomas's avatar
Thomas committed
257
{
258 259
	if (coarsenMark.isMarked(d) || refineMark.isMarked(d)) return ;
	if (totalNeighborSize(d) > nbAgentsToSimplify) return ;
Thomas's avatar
Thomas committed
260

261 262 263
	coarsenMark.mark(d) ;
	coarsenCandidate.push_back(map.faceOldestDart(d)) ;
}
Pierre Kraemer's avatar
Pierre Kraemer committed
264 265

inline void EnvMap::pushAgentInCells(Agent* agent, Dart d)
Pierre Kraemer's avatar
Pierre Kraemer committed
266
{
David Cazier's avatar
David Cazier committed
267
	assert(map.getCurrentLevel() == map.getMaxLevel()) ;
pitiot's avatar
maj  
pitiot committed
268
//	assert(std::find(agentvect[d].begin(), agentvect[d].end(), agent) == agentvect[d].end());
David Cazier's avatar
David Cazier committed
269

270 271
	addElementToVector<Agent*>(agentvect[d],agent);
//	agentvect[d].push_back(agent) ;
272
//	nbAgentsIncrease(d);
David Cazier's avatar
David Cazier committed
273 274

	Dart dd = d ;
David Cazier's avatar
David Cazier committed
275 276
	do
	{
David Cazier's avatar
David Cazier committed
277
		Dart ddd = map.alpha1(map.alpha1(dd)) ;
David Cazier's avatar
David Cazier committed
278 279
		while (ddd != dd)
		{
280 281
			addElementToVector<Agent*>(neighborAgentvect[ddd],agent);
//			neighborAgentvect[ddd].push_back(agent) ;
282
//			nbAgentsIncrease(ddd);
David Cazier's avatar
David Cazier committed
283
			ddd = map.alpha1(ddd) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
284
		}
David Cazier's avatar
David Cazier committed
285 286
		dd = map.phi1(dd) ;
	} while (dd != d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
287 288
}

Thomas's avatar
Thomas committed
289 290
inline void EnvMap::popAgentInCells(Agent* agent, Dart d)
{
David Cazier's avatar
David Cazier committed
291 292 293
	assert(map.getCurrentLevel() == map.getMaxLevel()) ;

	removeElementFromVector<Agent*>(agentvect[d], agent) ;
294
//	nbAgentsDecrease(d) ;
David Cazier's avatar
David Cazier committed
295 296

	Dart dd = d ;
David Cazier's avatar
David Cazier committed
297 298
	do
	{
David Cazier's avatar
David Cazier committed
299
		Dart ddd = map.alpha1(map.alpha1(dd)) ;
David Cazier's avatar
David Cazier committed
300 301
		while (ddd != dd)
		{
David Cazier's avatar
David Cazier committed
302
			removeElementFromVector<Agent*>(neighborAgentvect[ddd], agent) ;
303
//			nbAgentsDecrease(ddd) ;
David Cazier's avatar
David Cazier committed
304
			ddd = map.alpha1(ddd) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
305
		}
David Cazier's avatar
David Cazier committed
306 307
		dd = map.phi1(dd) ;
	} while (dd != d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
308 309
}

pitiot's avatar
maj  
pitiot committed
310 311 312

//ajout moving obst///////////////////////////////////////////////////////////////////////////////////////////////////////

313

314
inline void EnvMap::addObstAsNeighbor (Obstacle *  o, const std::vector<Dart>& belonging_cells, std::vector<Dart> *neighbor_cells)
pitiot's avatar
maj  
pitiot committed
315 316
{
	assert(map.getCurrentLevel() == map.getMaxLevel());
317
	assert(!belonging_cells.empty());
pitiot's avatar
maj  
pitiot committed
318
	neighbor_cells->clear();
319 320
	CellMarkerMemo<FACE> memo_mark(map);
	CellMarkerMemo<FACE> OneRingMark(map);
pitiot's avatar
maj  
pitiot committed
321

322 323
	for (std::vector<Dart>::const_iterator it =belonging_cells.begin();it<belonging_cells.end();++it)
		memo_mark.mark(*it);
pitiot's avatar
maj  
pitiot committed
324

325
	std::vector<Dart>::const_iterator it=belonging_cells.begin();
pitiot's avatar
maj  
pitiot committed
326 327 328 329
	Dart beg = NIL;
	Dart first =NIL;
	Dart d=NIL;
	Dart dd=NIL;
330
//	CGoGNout<<"beg : "<<(*it)<<CGoGNendl;
pitiot's avatar
maj  
pitiot committed
331 332 333 334 335 336 337 338 339
	//boucle pour trouver une face du voisinage de l'obstacle ne contenant pas l'obstacle
	do
	{
		beg = *it;
		first = NIL;
		d=beg;
		do {
			dd=map.alpha1(map.alpha1(d));
			do {
340
				if (!memo_mark.isMarked(dd))
pitiot's avatar
maj  
pitiot committed
341 342 343 344 345 346 347 348 349 350 351 352
				{
					first = dd;
				}
				dd=map.alpha1(dd);
			}while (first==NIL && dd!=d);

			d=map.phi1(d);

		} while(first==NIL && d!=beg);
		++it;
	}while(first==NIL && it!=belonging_cells.end());
	assert(!buildingMark.isMarked(d)) ;
353 354
	assert(first!=NIL) ;
//	CGoGNout<<"first : "<<first<<CGoGNendl;
pitiot's avatar
maj  
pitiot committed
355 356 357 358 359 360 361 362 363
	d=first;
	do
	{
		if (!OneRingMark.isMarked(d))
		{
			OneRingMark.mark(d);
			(*neighbor_cells).push_back(d);
		}

364
		find_next(o,&d, memo_mark);
365 366 367 368 369 370 371 372 373 374
		if(d==NIL)
		{
			CGoGNout<<"cellule de début : "<<first<<CGoGNendl;
			CGoGNout<<"cellules markées OneRing : "<<CGoGNendl;
			std::vector<Dart> v=OneRingMark.get_markedCells();
			for(std::vector<Dart>::iterator it=v.begin();it<v.end();++it)
			{
				CGoGNout<<(*it).index<<CGoGNendl;
			}
		}
375
//		CGoGNout<<"d : "<<d<<CGoGNendl;
pitiot's avatar
maj  
pitiot committed
376 377 378 379 380
	}while(!map.sameFace(d,first));
}

// find_next cherche la prochaine case "voisine" d'un obstacle faisant parti d'un movingobstacle (algo de parcours du one-ring )

381
inline void EnvMap::find_next(Obstacle* o,Dart * ddd, CellMarkerMemo<FACE>& cms)
pitiot's avatar
maj  
pitiot committed
382 383 384 385 386 387 388 389 390 391 392 393 394 395
{
	Dart d=*ddd;
	Dart first = NIL;
	Dart dd;
	int rd =0;
	do{
		rd++;
		dd=map.alpha1((d));

		if (!(cms.isMarked(dd)))
		{
			first = dd;
		}

396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417

		if(rd>100)
		{
			if (rd>10000)
			{
				displayMO(o);
				CGoGNout <<(first==NIL)<< " infini find next d : "<< d<<"phi 2 markée ? : "<<cms.isMarked(map.phi2(d))<<CGoGNendl;
				CGoGNout<<"cellules markées : "<<CGoGNendl;
				std::vector<Dart> v=cms.get_markedCells();
				for(std::vector<Dart>::iterator it=v.begin();it<v.end();++it)
				{
					CGoGNout<<(*it).index<<CGoGNendl;
				}
				break;
			}
			CGoGNout <<(first==NIL)<< " infini find next d : "<< d<<"phi 2 markée ? : "<<cms.isMarked(map.phi2(d))<<CGoGNendl;
			CGoGNout<<"cellules markées : "<<CGoGNendl;
							std::vector<Dart> v=cms.get_markedCells();
							for(std::vector<Dart>::iterator it=v.begin();it<v.end();++it)
							{
								CGoGNout<<(*it).index<<CGoGNendl;
							}
pitiot's avatar
maj  
pitiot committed
418
		}
419
		d=map.phi_1(d);
pitiot's avatar
maj  
pitiot committed
420 421 422 423 424 425 426 427
	}while(first==NIL);
	*ddd=first;
}

inline void EnvMap::pushObstNeighborInCells(Obstacle* o, Dart d)
{
	assert(map.getCurrentLevel() == map.getMaxLevel());

428
	addElementToVector<Obstacle*>(neighborObstvect[d],o);
pitiot's avatar
maj  
pitiot committed
429 430
}

431 432 433 434
//inline void EnvMap::pushObstacleInCell(Obstacle* o, Dart d)
//{
//	assert(map.getCurrentLevel() == map.getMaxLevel()) ;
//	assert(!buildingMark.isMarked(d)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
435
//
436 437
//	addElementToVector<Obstacle*>(obstvect[d],o);
//}
438

Pierre Kraemer's avatar
Pierre Kraemer committed
439 440
inline void EnvMap::clearUpdateCandidates()
{
David Cazier's avatar
David Cazier committed
441 442
	refineCandidate.clear() ;
	coarsenCandidate.clear() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
443
}
pitiot's avatar
maj  
pitiot committed
444 445
///addMovingObstAsNeighbor est pour détecter les voisins du movingobstacle global afin de le considerer comme un super agent

Pierre Kraemer's avatar
Pierre Kraemer committed
446
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
447 448

#endif