env_map.h 6.04 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"

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

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 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
48
49
50

class EnvMap
{
David Cazier's avatar
David Cazier committed
51
52
	public:
	EnvMap() ;
53

David Cazier's avatar
David Cazier committed
54
	void markPedWay() ;
Thomas's avatar
Thomas committed
55

David Cazier's avatar
David Cazier committed
56
57
	unsigned int mapMemoryCost() ;
	void scale(float scaleVal) ;
Thomas's avatar
Thomas committed
58

Pierre Kraemer's avatar
Pierre Kraemer committed
59
#ifndef SPATIAL_HASHING
Pierre Kraemer's avatar
Pierre Kraemer committed
60
	Dart getBelongingCell(const PFP::VEC3& pos);
Pierre Kraemer's avatar
Pierre Kraemer committed
61

Thomas's avatar
Thomas committed
62
63
	void subdivideAllToMaxLevel();
	void subdivideToProperLevel();
Pierre Kraemer's avatar
Pierre Kraemer committed
64
#endif
David Cazier's avatar
David Cazier committed
65

Thomas's avatar
Thomas committed
66
	void init(unsigned int config);
David Cazier's avatar
David Cazier committed
67

Pierre Kraemer's avatar
Pierre Kraemer committed
68
#ifndef SPATIAL_HASHING
Pierre Kraemer's avatar
Pierre Kraemer committed
69
	void foreach_neighborFace(Dart d, FunctorType& f);
David Cazier's avatar
David Cazier committed
70

Pierre Kraemer's avatar
Pierre Kraemer committed
71
72
	void registerObstaclesInFaces();
	void addNeighborObstacles(PFP::OBSTACLES& obst, Dart d, bool edgeNeighbor);
David Cazier's avatar
David Cazier committed
73

Pierre Kraemer's avatar
Pierre Kraemer committed
74
	void setAgentNeighborsAndObstacles(Agent* agent);
75

Pierre Kraemer's avatar
Pierre Kraemer committed
76
77
	void pushAgentInCells(Agent* agent, Dart d);
	void popAgentInCells(Agent* agent, Dart d);
78
//	void agentChangeFaceThroughEdge(Agent* agent);
Pierre Kraemer's avatar
Pierre Kraemer committed
79
	void agentChangeFace(Agent* agent, Dart oldFace);
David Cazier's avatar
David Cazier committed
80

Pierre Kraemer's avatar
Pierre Kraemer committed
81
82
83
	void pushObstacleInCells(Obstacle* o, Dart d);
	void popObstacleInCells(Obstacle* o, Dart d);
	void obstacleChangeFace(Obstacle* agent, Dart newFace, Dart oldFace);
Pierre Kraemer's avatar
Pierre Kraemer committed
84
85

	void resetAgentInFace(Agent* agent);
Pierre Kraemer's avatar
Pierre Kraemer committed
86
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
87
88
89

	PFP::MAP map;

David Cazier's avatar
David Cazier committed
90
91
	PFP::TVEC3 position ;
	PFP::TVEC3 normal ;
Pierre Kraemer's avatar
Pierre Kraemer committed
92

David Cazier's avatar
David Cazier committed
93
94
95
96
97
	PFP::MAP mapScenary ;
	PFP::TVEC3 positionScenary ;
	PFP::TVEC3 normalScenary ;
	CellMarker obstacleMarkS ;
	CellMarker buildingMarkS ;
98

David Cazier's avatar
David Cazier committed
99
	std::vector<Dart> newBuildings ;
100

Pierre Kraemer's avatar
Pierre Kraemer committed
101
102
103
#ifndef SPATIAL_HASHING
	AttributeHandler<PFP::BOOLATTRIB> subdivisableFace;

Pierre Kraemer's avatar
Pierre Kraemer committed
104
	PFP::TAB_AGENTVECT agentvect;
Pierre Kraemer's avatar
Pierre Kraemer committed
105
106
	PFP::TAB_AGENTVECT neighborAgentvect;
	
Pierre Kraemer's avatar
Pierre Kraemer committed
107
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
108

David Cazier's avatar
David Cazier committed
109
	PFP::TAB_OBSTACLEVECT obstvect ;
Pierre Kraemer's avatar
Pierre Kraemer committed
110

David Cazier's avatar
David Cazier committed
111
112
113
	CellMarker obstacleMark ;
	CellMarker buildingMark ;
	CellMarker pedWayMark ;
Pierre Kraemer's avatar
Pierre Kraemer committed
114

Pierre Kraemer's avatar
Pierre Kraemer committed
115
#ifndef SPATIAL_HASHING
Pierre Kraemer's avatar
Pierre Kraemer committed
116
117
	static const unsigned int nbAgentsToSubdivide = 5;
	static const unsigned int nbAgentsToSimplify = 4;
David Cazier's avatar
David Cazier committed
118

119
	CellMarker refineMark;
Pierre Kraemer's avatar
Pierre Kraemer committed
120
	CellMarker coarsenMark;
121
	std::vector<Dart> refineCandidate;
Pierre Kraemer's avatar
Pierre Kraemer committed
122
	std::vector<Dart> coarsenCandidate;
Pierre Kraemer's avatar
Pierre Kraemer committed
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
#endif

#ifdef SPATIAL_HASHING
	PFP::VEC3 origin ;

	unsigned int a_cell_size_x, a_cell_size_y ;
	float a_cell_nb_x, a_cell_nb_y ;

	unsigned int o_cell_size_x, o_cell_size_y ;
	float o_cell_nb_x, o_cell_nb_y ;

	Geom::Vec2ui agentPositionCell(VEC3& pos)
	{
		return Geom::Vec2ui((origin[0] + pos[0]) / a_cell_size_x, (origin[1] + pos[1]) / a_cell_size_y) ;
	}

	Geom::Vec2ui obstaclePositionCell(VEC3& pos)
	{
		return Geom::Vec2ui((origin[0] + pos[0]) / o_cell_size_x, (origin[1] + pos[1]) / o_cell_size_y) ;
	}

	HashTable2D< std::vector<Agent*> > ht_agents ;
	HashTable2D< std::vector<Agent*> > ht_neighbor_agents ;
	HashTable2D< std::vector<Obstacle*> > ht_obstacles ;
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
148
149
150
};

/**************************************
David Cazier's avatar
David Cazier committed
151
152
 *           INLINE FUNCTIONS          *
 **************************************/
Pierre Kraemer's avatar
Pierre Kraemer committed
153

Pierre Kraemer's avatar
Pierre Kraemer committed
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
template <typename T>
inline void removeElementFromVector(std::vector<T>& a, T ag)
{
	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;
		}
	}
}

#ifndef SPATIAL_HASHING
Pierre Kraemer's avatar
Pierre Kraemer committed
170
inline void EnvMap::pushAgentInCells(Agent* agent, Dart d)
Pierre Kraemer's avatar
Pierre Kraemer committed
171
{
David Cazier's avatar
David Cazier committed
172
173
174
175
176
177
178
179
180
181
182
	assert(map.getCurrentLevel() == map.getMaxLevel()) ;
	assert(std::find(agentvect[d].begin(), agentvect[d].end(), agent) == agentvect[d].end()) ;

	agentvect[d].push_back(agent) ;

	Dart dd = d ;
	do {
		Dart ddd = map.alpha1(map.alpha1(dd)) ;
		while (ddd != dd) {
			neighborAgentvect[ddd].push_back(agent) ;
			ddd = map.alpha1(ddd) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
183
		}
David Cazier's avatar
David Cazier committed
184
185
		dd = map.phi1(dd) ;
	} while (dd != d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
186
187
}

Thomas's avatar
Thomas committed
188
189
inline void EnvMap::popAgentInCells(Agent* agent, Dart d)
{
David Cazier's avatar
David Cazier committed
190
191
192
193
194
195
196
197
198
199
200
	assert(map.getCurrentLevel() == map.getMaxLevel()) ;
	assert(std::find(agentvect[d].begin(), agentvect[d].end(), agent) != agentvect[d].end()) ;

	removeElementFromVector<Agent*>(agentvect[d], agent) ;

	Dart dd = d ;
	do {
		Dart ddd = map.alpha1(map.alpha1(dd)) ;
		while (ddd != dd) {
			removeElementFromVector<Agent*>(neighborAgentvect[ddd], agent) ;
			ddd = map.alpha1(ddd) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
201
		}
David Cazier's avatar
David Cazier committed
202
203
		dd = map.phi1(dd) ;
	} while (dd != d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
204
205
}

Pierre Kraemer's avatar
Pierre Kraemer committed
206
inline void EnvMap::pushObstacleInCells(Obstacle* o, Dart d)
207
{
Pierre Kraemer's avatar
Pierre Kraemer committed
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
	assert(map.getCurrentLevel() == map.getMaxLevel());
	assert(!buildingMark.isMarked(d));

	obstvect[d].push_back(o);

//	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)
{
	assert(map.getCurrentLevel() == map.getMaxLevel());
	assert(!buildingMark.isMarked(d));

	removeElementFromVector<Obstacle* >(obstvect[d], o);

//	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);
251
252
}

Pierre Kraemer's avatar
Pierre Kraemer committed
253
254
inline void EnvMap::clearUpdateCandidates()
{
David Cazier's avatar
David Cazier committed
255
256
	refineCandidate.clear() ;
	coarsenCandidate.clear() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
257
}
Pierre Kraemer's avatar
Pierre Kraemer committed
258
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
259
260

#endif