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