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