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