env_map.h 7.34 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
54
55
56
57
58
59
60
61
62
63
64
65
66
public:
	PFP::MAP map ;

	/* 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) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
76

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

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

84
85
	void registerObstaclesInFaces() ;
	void addNeighborObstacles(PFP::OBSTACLES& obst, Dart d, bool edgeNeighbor) ;
David Cazier's avatar
David Cazier committed
86

87
	void setAgentNeighborsAndObstacles(Agent* agent) ;
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) ;
94
//	void agentChangeFaceThroughEdge(Agent* agent);
95
	void agentChangeFace(Agent* agent, Dart oldFace) ;
David Cazier's avatar
David Cazier 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() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
101

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

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

119
120
	PFP::TAB_AGENTVECT agentvect ;
	PFP::TAB_AGENTVECT neighborAgentvect ;
Pierre Kraemer's avatar
Pierre Kraemer committed
121
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
122

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

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

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

133
134
135
136
137
138
	CellMarker refineMark ;
	CellMarker coarsenMark ;
	std::vector<Dart> refineCandidate ;
	std::vector<Dart> coarsenCandidate ;

	void clearUpdateCandidates() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
139
140
141
142
143
144
145
146
#endif

#ifdef SPATIAL_HASHING
	float a_cell_nb_x, a_cell_nb_y ;
	float o_cell_nb_x, o_cell_nb_y ;

	Geom::Vec2ui agentPositionCell(VEC3& pos)
	{
147
148
149
		VEC3 relativePos = pos - geometry.min();
		relativePos /= minCellSize;
		return Geom::Vec2ui(relativePos[0], relativePos[1]) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
150
151
152
153
	}

	Geom::Vec2ui obstaclePositionCell(VEC3& pos)
	{
154
155
156
		VEC3 relativePos = pos - geometry.min();
		relativePos /= obstacleDistance;
		return Geom::Vec2ui(relativePos[0], relativePos[1]) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
157
158
159
160
161
162
	}

	HashTable2D< std::vector<Agent*> > ht_agents ;
	HashTable2D< std::vector<Agent*> > ht_neighbor_agents ;
	HashTable2D< std::vector<Obstacle*> > ht_obstacles ;
#endif
163
} ;
Pierre Kraemer's avatar
Pierre Kraemer committed
164
165

/**************************************
David Cazier's avatar
David Cazier committed
166
167
 *           INLINE FUNCTIONS          *
 **************************************/
Pierre Kraemer's avatar
Pierre Kraemer committed
168

169
template<typename T>
Pierre Kraemer's avatar
Pierre Kraemer committed
170
171
inline void removeElementFromVector(std::vector<T>& a, T ag)
{
172
173
174
175
176
177
	typename std::vector<T>::iterator end = a.template end() ;
	for (typename std::vector<T>::iterator it = a.begin() ; it != end ; ++it) {
		if ( *it == ag) {
			*it = a.back() ;
			a.pop_back() ;
			return ;
Pierre Kraemer's avatar
Pierre Kraemer committed
178
179
180
181
182
		}
	}
}

#ifndef SPATIAL_HASHING
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
inline unsigned int EnvMap::totalNeighborSize(Dart d) {
	return agentvect[d].size() + neighborAgentvect[d].size() ;
}

inline void EnvMap::nbAgentsIncrease(Dart d)
{
	if (refineMark.isMarked(d)) return ;
	if (totalNeighborSize(d) < nbAgentsToSubdivide) return ;

	std::pair<bool, bool>& sf = subdivisableFace[d] ;
	if (sf.first == false || (sf.first == true && sf.second)) {
		refineMark.mark(d) ;
		refineCandidate.push_back(d) ;
	}
}

inline void EnvMap::nbAgentsDecrease(Dart d)
{
	if (coarsenMark.isMarked(d) || refineMark.isMarked(d)) return ;
	if (totalNeighborSize(d) > nbAgentsToSimplify) return ;

	coarsenMark.mark(d) ;
	coarsenCandidate.push_back(map.faceOldestDart(d)) ;
}

Pierre Kraemer's avatar
Pierre Kraemer committed
208
inline void EnvMap::pushAgentInCells(Agent* agent, Dart d)
Pierre Kraemer's avatar
Pierre Kraemer committed
209
{
David Cazier's avatar
David Cazier committed
210
211
212
	assert(map.getCurrentLevel() == map.getMaxLevel()) ;

	agentvect[d].push_back(agent) ;
213
//	nbAgentsIncrease(d);
David Cazier's avatar
David Cazier committed
214
215
216
217
218
219

	Dart dd = d ;
	do {
		Dart ddd = map.alpha1(map.alpha1(dd)) ;
		while (ddd != dd) {
			neighborAgentvect[ddd].push_back(agent) ;
220
//			nbAgentsIncrease(ddd);
David Cazier's avatar
David Cazier committed
221
			ddd = map.alpha1(ddd) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
222
		}
David Cazier's avatar
David Cazier committed
223
224
		dd = map.phi1(dd) ;
	} while (dd != d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
225
226
}

Thomas's avatar
Thomas committed
227
228
inline void EnvMap::popAgentInCells(Agent* agent, Dart d)
{
David Cazier's avatar
David Cazier committed
229
230
231
	assert(map.getCurrentLevel() == map.getMaxLevel()) ;

	removeElementFromVector<Agent*>(agentvect[d], agent) ;
232
//	nbAgentsDecrease(d) ;
David Cazier's avatar
David Cazier committed
233
234
235
236
237
238

	Dart dd = d ;
	do {
		Dart ddd = map.alpha1(map.alpha1(dd)) ;
		while (ddd != dd) {
			removeElementFromVector<Agent*>(neighborAgentvect[ddd], agent) ;
239
//			nbAgentsDecrease(ddd) ;
David Cazier's avatar
David Cazier committed
240
			ddd = map.alpha1(ddd) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
241
		}
David Cazier's avatar
David Cazier committed
242
243
		dd = map.phi1(dd) ;
	} while (dd != d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
244
245
}

Pierre Kraemer's avatar
Pierre Kraemer committed
246
inline void EnvMap::pushObstacleInCells(Obstacle* o, Dart d)
247
{
248
249
	assert(map.getCurrentLevel() == map.getMaxLevel()) ;
	assert( !buildingMark.isMarked(d)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
250

251
	obstvect[d].push_back(o) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271

//	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)
{
272
273
	assert(map.getCurrentLevel() == map.getMaxLevel()) ;
	assert( !buildingMark.isMarked(d)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
274

275
	removeElementFromVector<Obstacle*>(obstvect[d], o) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290

//	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);
291
292
}

Pierre Kraemer's avatar
Pierre Kraemer committed
293
294
inline void EnvMap::clearUpdateCandidates()
{
David Cazier's avatar
David Cazier committed
295
296
	refineCandidate.clear() ;
	coarsenCandidate.clear() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
297
}
Pierre Kraemer's avatar
Pierre Kraemer committed
298
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
299
300

#endif