env_map.h 7.47 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

David Cazier's avatar
David Cazier committed
28 29
class Agent ;
class Obstacle ;
Pierre Kraemer's avatar
Pierre Kraemer committed
30

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

	// definition des listes d'agent
David Cazier's avatar
David Cazier committed
37 38 39 40 41 42
	typedef std::vector<Agent*> AGENTS ;
	typedef std::vector<Obstacle*> OBSTACLES ;
	typedef NoMathIONameAttribute<AGENTS> AGENTVECT ;
	typedef NoMathIONameAttribute<OBSTACLES> OBSTACLEVECT ;
	typedef AttributeHandler<AGENTVECT> TAB_AGENTVECT ;
	typedef AttributeHandler<OBSTACLEVECT> TAB_OBSTACLEVECT ;
43

David Cazier's avatar
David Cazier committed
44 45
	typedef NoMathIONameAttribute<std::pair<bool, bool> > BOOLATTRIB ;
} ;
Pierre Kraemer's avatar
Pierre Kraemer committed
46

David Cazier's avatar
David Cazier committed
47
typedef PFP::VEC3 VEC3 ;
48
typedef PFP::REAL REAL ;
Pierre Kraemer's avatar
Pierre Kraemer committed
49 50 51

class EnvMap
{
52 53
public:
	PFP::MAP map ;
54

55 56 57 58 59 60 61 62 63 64 65 66
	/* 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
67
	EnvMap() ;
68
	void init(unsigned int config, REAL width, REAL height, REAL minSize, REAL maxSize) ;
69

David Cazier's avatar
David Cazier committed
70
	void markPedWay() ;
Thomas's avatar
Thomas committed
71

David Cazier's avatar
David Cazier committed
72
	unsigned int mapMemoryCost() ;
Thomas's avatar
Thomas committed
73

Pierre Kraemer's avatar
Pierre Kraemer committed
74
#ifndef SPATIAL_HASHING
75
	Dart getBelongingCell(const VEC3& pos) ;
Thomas's avatar
Thomas committed
76

77 78
	void subdivideAllToMaxLevel() ;
	void subdivideToProperLevel() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
79
#endif
Thomas's avatar
Thomas committed
80

Pierre Kraemer's avatar
Pierre Kraemer committed
81
#ifndef SPATIAL_HASHING
82
	void foreach_neighborFace(Dart d, FunctorType& f) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
83

84 85
	void registerObstaclesInFaces() ;
	void addNeighborObstacles(PFP::OBSTACLES& obst, Dart d, bool edgeNeighbor) ;
86

87
	void setAgentNeighborsAndObstacles(Agent* agent) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
88

89 90 91 92 93
	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) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
94
//	void agentChangeFaceThroughEdge(Agent* agent);
95
	void agentChangeFace(Agent* agent, Dart oldFace) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
96

97 98 99 100
	void pushObstacleInCells(Obstacle* o, Dart d) ;
	void popObstacleInCells(Obstacle* o, Dart d) ;
	void obstacleChangeFace(Obstacle* agent, Dart newFace, Dart oldFace) ;
	void updateMap() ;
101

102
	void resetAgentInFace(Agent* agent) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
103 104
#endif

David Cazier's avatar
David Cazier committed
105 106
	PFP::TVEC3 position ;
	PFP::TVEC3 normal ;
Pierre Kraemer's avatar
Pierre Kraemer committed
107

David Cazier's avatar
David Cazier committed
108 109 110 111 112
	PFP::MAP mapScenary ;
	PFP::TVEC3 positionScenary ;
	PFP::TVEC3 normalScenary ;
	CellMarker obstacleMarkS ;
	CellMarker buildingMarkS ;
113

David Cazier's avatar
David Cazier committed
114
	std::vector<Dart> newBuildings ;
115

Pierre Kraemer's avatar
Pierre Kraemer committed
116
#ifndef SPATIAL_HASHING
117
	AttributeHandler<PFP::BOOLATTRIB> subdivisableFace ;
Pierre Kraemer's avatar
Pierre Kraemer committed
118
	PFP::TAB_AGENTVECT agentvect;
Pierre Kraemer's avatar
Pierre Kraemer committed
119
	PFP::TAB_AGENTVECT neighborAgentvect;
Pierre Kraemer's avatar
Pierre Kraemer committed
120
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
121

David Cazier's avatar
David Cazier committed
122
	PFP::TAB_OBSTACLEVECT obstvect ;
Pierre Kraemer's avatar
Pierre Kraemer committed
123

David Cazier's avatar
David Cazier committed
124 125 126
	CellMarker obstacleMark ;
	CellMarker buildingMark ;
	CellMarker pedWayMark ;
Pierre Kraemer's avatar
Pierre Kraemer committed
127

Pierre Kraemer's avatar
Pierre Kraemer committed
128
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
129 130
	static const unsigned int nbAgentsToSubdivide = 10 ;
	static const unsigned int nbAgentsToSimplify = 4 ;
David Cazier's avatar
David Cazier committed
131

132 133 134 135
	CellMarker refineMark ;
	CellMarker coarsenMark ;
	std::vector<Dart> refineCandidate ;
	std::vector<Dart> coarsenCandidate ;
Pierre Kraemer's avatar
Pierre Kraemer committed
136

137
	void clearUpdateCandidates() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
138 139 140
#endif

#ifdef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
141 142 143 144 145 146 147 148 149
	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
150 151 152

	Geom::Vec2ui agentPositionCell(VEC3& pos)
	{
David Cazier's avatar
David Cazier committed
153 154
		VEC3 relativePos = pos - geometry.min() ;
		relativePos /= minCellSize ;
155
		return Geom::Vec2ui(relativePos[0], relativePos[1]) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
156 157 158 159
	}

	Geom::Vec2ui obstaclePositionCell(VEC3& pos)
	{
David Cazier's avatar
David Cazier committed
160 161
		VEC3 relativePos = pos - geometry.min() ;
		relativePos /= obstacleDistance ;
162
		return Geom::Vec2ui(relativePos[0], relativePos[1]) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
163 164 165 166 167 168
	}

	HashTable2D< std::vector<Agent*> > ht_agents ;
	HashTable2D< std::vector<Agent*> > ht_neighbor_agents ;
	HashTable2D< std::vector<Obstacle*> > ht_obstacles ;
#endif
169
} ;
Pierre Kraemer's avatar
Pierre Kraemer committed
170 171

/**************************************
David Cazier's avatar
David Cazier committed
172 173
 *           INLINE FUNCTIONS          *
 **************************************/
Pierre Kraemer's avatar
Pierre Kraemer committed
174

David Cazier's avatar
David Cazier committed
175
template <typename T>
Pierre Kraemer's avatar
Pierre Kraemer committed
176 177
inline void removeElementFromVector(std::vector<T>& a, T ag)
{
178
	typename std::vector<T>::iterator end = a.template end() ;
David Cazier's avatar
David Cazier committed
179 180 181 182
	for (typename std::vector<T>::iterator it = a.begin(); it != end; ++it)
	{
		if (*it == ag)
		{
183 184 185
			*it = a.back() ;
			a.pop_back() ;
			return ;
Pierre Kraemer's avatar
Pierre Kraemer committed
186 187 188 189 190
		}
	}
}

#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
191 192
inline unsigned int EnvMap::totalNeighborSize(Dart d)
{
193 194 195 196
	return agentvect[d].size() + neighborAgentvect[d].size() ;
}

inline void EnvMap::nbAgentsIncrease(Dart d)
Pierre Kraemer's avatar
Pierre Kraemer committed
197
{
198 199
	if (refineMark.isMarked(d)) return ;
	if (totalNeighborSize(d) < nbAgentsToSubdivide) return ;
Pierre Kraemer's avatar
Pierre Kraemer committed
200

201
	std::pair<bool, bool>& sf = subdivisableFace[d] ;
David Cazier's avatar
David Cazier committed
202 203
	if (sf.first == false || (sf.first == true && sf.second))
	{
204 205 206
		refineMark.mark(d) ;
		refineCandidate.push_back(d) ;
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
207 208
}

209
inline void EnvMap::nbAgentsDecrease(Dart d)
Thomas's avatar
Thomas committed
210
{
211 212
	if (coarsenMark.isMarked(d) || refineMark.isMarked(d)) return ;
	if (totalNeighborSize(d) > nbAgentsToSimplify) return ;
Thomas's avatar
Thomas committed
213

214 215 216
	coarsenMark.mark(d) ;
	coarsenCandidate.push_back(map.faceOldestDart(d)) ;
}
Pierre Kraemer's avatar
Pierre Kraemer committed
217 218

inline void EnvMap::pushAgentInCells(Agent* agent, Dart d)
Pierre Kraemer's avatar
Pierre Kraemer committed
219
{
David Cazier's avatar
David Cazier committed
220 221 222
	assert(map.getCurrentLevel() == map.getMaxLevel()) ;

	agentvect[d].push_back(agent) ;
223
//	nbAgentsIncrease(d);
David Cazier's avatar
David Cazier committed
224 225

	Dart dd = d ;
David Cazier's avatar
David Cazier committed
226 227
	do
	{
David Cazier's avatar
David Cazier committed
228
		Dart ddd = map.alpha1(map.alpha1(dd)) ;
David Cazier's avatar
David Cazier committed
229 230
		while (ddd != dd)
		{
David Cazier's avatar
David Cazier committed
231
			neighborAgentvect[ddd].push_back(agent) ;
232
//			nbAgentsIncrease(ddd);
David Cazier's avatar
David Cazier committed
233
			ddd = map.alpha1(ddd) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
234
		}
David Cazier's avatar
David Cazier committed
235 236
		dd = map.phi1(dd) ;
	} while (dd != d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
237 238
}

Thomas's avatar
Thomas committed
239 240
inline void EnvMap::popAgentInCells(Agent* agent, Dart d)
{
David Cazier's avatar
David Cazier committed
241 242 243
	assert(map.getCurrentLevel() == map.getMaxLevel()) ;

	removeElementFromVector<Agent*>(agentvect[d], agent) ;
244
//	nbAgentsDecrease(d) ;
David Cazier's avatar
David Cazier committed
245 246

	Dart dd = d ;
David Cazier's avatar
David Cazier committed
247 248
	do
	{
David Cazier's avatar
David Cazier committed
249
		Dart ddd = map.alpha1(map.alpha1(dd)) ;
David Cazier's avatar
David Cazier committed
250 251
		while (ddd != dd)
		{
David Cazier's avatar
David Cazier committed
252
			removeElementFromVector<Agent*>(neighborAgentvect[ddd], agent) ;
253
//			nbAgentsDecrease(ddd) ;
David Cazier's avatar
David Cazier committed
254
			ddd = map.alpha1(ddd) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
255
		}
David Cazier's avatar
David Cazier committed
256 257
		dd = map.phi1(dd) ;
	} while (dd != d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
258 259
}

Pierre Kraemer's avatar
Pierre Kraemer committed
260
inline void EnvMap::pushObstacleInCells(Obstacle* o, Dart d)
261
{
262
	assert(map.getCurrentLevel() == map.getMaxLevel()) ;
David Cazier's avatar
David Cazier committed
263
	assert(!buildingMark.isMarked(d)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
264

265
	obstvect[d].push_back(o) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285

//	obstvect[map.phi<12>(d)].push_back(o);
//	obstvect[map.phi2(map.phi_1(d))].push_back(o);

//	Dart dd = d;
//	do
//	{
//		Dart ddd = map.alpha1(map.alpha1(dd));
//		while(ddd != dd)
//		{
//			obstvect[ddd].push_back(o);
//			ddd = map.alpha1(ddd);
//		}
//
//		dd = map.phi1(dd);
//	} while(dd != d);
}

inline void EnvMap::popObstacleInCells(Obstacle* o, Dart d)
{
286
	assert(map.getCurrentLevel() == map.getMaxLevel()) ;
David Cazier's avatar
David Cazier committed
287
	assert(!buildingMark.isMarked(d)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
288

289
	removeElementFromVector<Obstacle*>(obstvect[d], o) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
290 291 292 293 294 295 296 297 298 299 300 301 302 303 304

//	removeElementFromVector<Obstacle* >(obstvect[map.phi<12>(d)],o);
//	removeElementFromVector<Obstacle* >(obstvect[map.phi2(map.phi_1(d))],o);

//	Dart dd = d;
//	do
//	{
//		Dart ddd = map.alpha1(map.alpha1(dd));
//		while(ddd != dd)
//		{
//			removeElementFromVector<Obstacle* >(obstvect[ddd], o);
//			ddd = map.alpha1(ddd);
//		}
//		dd = map.phi1(dd);
//	} while(dd != d);
305 306
}

Pierre Kraemer's avatar
Pierre Kraemer committed
307 308
inline void EnvMap::clearUpdateCandidates()
{
David Cazier's avatar
David Cazier committed
309 310
	refineCandidate.clear() ;
	coarsenCandidate.clear() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
311
}
Pierre Kraemer's avatar
Pierre Kraemer committed
312
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
313 314

#endif