env_map.cpp 11.5 KB
Newer Older
Pierre Kraemer's avatar
Pierre Kraemer committed
1
2
3
4
#include "env_map.h"
#include "utils.h"
#include "agent.h"

Pierre Kraemer's avatar
Pierre Kraemer committed
5
6
#include "Geometry/inclusion.h"
#include "Algo/MovingObjects/particle_cell_2D_memo.h"
Pierre Kraemer's avatar
Pierre Kraemer committed
7
8
9
10
11
12
13
14
15
16
#include "Algo/Modelisation/subdivision.h"
#include "Algo/Geometry/normal.h"

using namespace CGoGN;

EnvMap::EnvMap() : obstacleMark(map, EDGE_CELL), buildingMark(map, FACE_CELL)
{
	position = map.addAttribute<PFP::VEC3>(VERTEX_ORBIT, "position");
	normal = map.addAttribute<PFP::VEC3>(VERTEX_ORBIT, "normal");
	agentvect = map.addAttribute<PFP::AGENTVECT>(FACE_ORBIT, "agents");
Pierre Kraemer's avatar
Pierre Kraemer committed
17
	neighborAgentvect = map.addAttribute<PFP::AGENTVECT>(FACE_ORBIT, "neighborAgents");
Pierre Kraemer's avatar
Pierre Kraemer committed
18
	obstvect = map.addAttribute<PFP::OBSTACLEVECT>(FACE_ORBIT, "obstacles");
19
	subdivisableFace = map.addAttribute<PFP::BOOLATTRIB>(FACE_ORBIT, "subdivisableFace");
Pierre Kraemer's avatar
Pierre Kraemer committed
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
}

unsigned int EnvMap::mapMemoryCost()
{
	return (map.getAttributeContainer(DART_ORBIT)).memorySize()
			+ (map.getAttributeContainer(VERTEX_ORBIT)).memorySize()
			+ (map.getAttributeContainer(EDGE_ORBIT)).memorySize()
			+ (map.getAttributeContainer(FACE_ORBIT)).memorySize();
}

Dart EnvMap::getBelongingCell(const PFP::VEC3& pos)
{
	for(Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if(!buildingMark.isMarked(d) && Algo::Geometry::isPointInConvexFace2D<PFP>(map, d, position, pos, true))
			return d;
	}

	std::cout << "ERROR : pos not in map" << std::endl;
	return map.begin();
}

void EnvMap::init()
{
	float sideSize = 70.0f;
Pierre Kraemer's avatar
Pierre Kraemer committed
45
	unsigned int nbSquares = 20;
Pierre Kraemer's avatar
Pierre Kraemer committed
46
	CityGenerator::generateCity<PFP>(map, position, obstacleMark, buildingMark, sideSize, nbSquares);
47
//	CityGenerator::generateMall<PFP>(map, position, obstacleMark, buildingMark, sideSize);
Pierre Kraemer's avatar
Pierre Kraemer committed
48
//	CityGenerator::simplifyFreeSpace<PFP>(map, position, obstacleMark, buildingMark);
Pierre Kraemer's avatar
Pierre Kraemer committed
49
50
	map.init();
	registerObstaclesInFaces();
51
52
	for(unsigned int i = subdivisableFace.begin(); i < subdivisableFace.end(); subdivisableFace.next(i))
		subdivisableFace[i].first = false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
53
54
}

Pierre Kraemer's avatar
Pierre Kraemer committed
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
void EnvMap::foreach_neighborFace(Dart d, FunctorType& f)
{
	Dart dd = d;
	do
	{
		Dart ddd = map.alpha1(map.alpha1(dd));
		while(ddd != dd)
		{
			f(ddd);
			ddd = map.alpha1(ddd);
		}
		dd = map.phi1(dd);
	} while(dd != d);
}

Pierre Kraemer's avatar
Pierre Kraemer committed
70
71
72
73
74
75
76
77
void EnvMap::registerObstaclesInFaces()
{
	CellMarker m(map, FACE_CELL);
	for(Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if(!m.isMarked(d))
		{
			m.mark(d);
78
			if(!buildingMark.isMarked(d))
Pierre Kraemer's avatar
Pierre Kraemer committed
79
			{
80
81
82
83
84
85
86
				Dart dd = d;
				do
				{
					if(obstacleMark.isMarked(dd))
						obstvect[d].push_back(map.phi2(dd));
					dd = map.phi1(dd);
				} while(dd != d);
Pierre Kraemer's avatar
Pierre Kraemer committed
87

88
				do
Pierre Kraemer's avatar
Pierre Kraemer committed
89
				{
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
					Dart ddd = map.alpha1(map.alpha1(dd));
					while(ddd != dd)
					{
						if(!buildingMark.isMarked(ddd))
							addNeighborObstacles(obstvect[d], ddd, ddd == map.alpha_1(dd));
						ddd = map.alpha1(ddd);
					}
					dd = map.phi1(dd);
				} while(dd != d);
			}
		}
	}

	for(Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if(m.isMarked(d) && !buildingMark.isMarked(d)) {
			m.unmark(d);
Pierre Kraemer's avatar
Pierre Kraemer committed
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
		}
	}
}

void EnvMap::addNeighborObstacles(PFP::OBSTACLES& obst, Dart d, bool edgeNeighbor)
{
	Dart stop;
	if(edgeNeighbor)
		stop = map.phi_1(map.phi_1(map.phi_1(d)));
	else
		stop = map.phi_1(map.phi_1(d));
	
	Dart dd = d;
	do
	{
		if(obstacleMark.isMarked(dd))
		{
			if(buildingMark.isMarked(dd))
				obst.push_back(dd);
			else
				obst.push_back(map.phi2(dd));
		}
		dd = map.phi1(dd);
	} while(dd != d);
}

133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
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
208
209
//void EnvMap::agentChangeFaceThroughEdge(Agent* agent)
//{
//	Dart oldFace = agent->part_.lastCrossed;
//	Dart newFace = agent->part_.d;
//
//	agentvect[newFace].push_back(agent);
//
//	PFP::AGENTS::iterator end = agentvect[oldFace].end();
//	for(PFP::AGENTS::iterator it = agentvect[oldFace].begin(); it != end; ++it)
//	{
//		if(*it == agent)
//		{
//			*it = agentvect[oldFace].back();
//			agentvect[oldFace].pop_back();
//			break;
//		}
//	}
//
//	// mark adjacent cells shared by newFace and oldFace
//	CellMarkerNoUnmark f(map, FACE_CELL);
//	Dart d = oldFace;
//	do
//	{
//		f.mark(d);
//		d = map.alpha1(d);
//	} while(d != oldFace);
//
//	d = map.phi1(oldFace);
//	do
//	{
//		f.mark(d);
//		d = map.alpha1(d);
//	} while(d != map.phi1(oldFace));
//
//	// remove agent from cells adjacent to oldFace but not to newFace
//	Dart dd = oldFace;
//	do
//	{
//		Dart ddd = map.alpha1(map.alpha1(dd));
//		while(ddd != dd)
//		{
//			if(!f.isMarked(ddd))
//			{
//				bool found = false;
//				PFP::AGENTS::iterator end = neighborAgentvect[ddd].end();
//				for(PFP::AGENTS::iterator it = neighborAgentvect[ddd].begin(); it != end && !found; ++it)
//				{
//					if(*it == agent)
//					{
//						*it = neighborAgentvect[ddd].back();
//						neighborAgentvect[ddd].pop_back();
//						found = true;
//					}
//				}
//			}
//			ddd = map.alpha1(ddd);
//		}
//		dd = map.phi1(dd);
//	} while(dd != oldFace);
//
//	// add agent from cells adjacent to oldFace but not to newFace
//	// and unmark shared cells
//	dd = newFace;
//	do
//	{
//		Dart ddd = map.alpha1(map.alpha1(dd));
//		while(ddd != dd)
//		{
//			if(!f.isMarked(ddd))
//				neighborAgentvect[ddd].push_back(agent);
//			else
//				f.unmark(ddd);
//			ddd = map.alpha1(ddd);
//		}
//		dd = map.phi1(dd);
//	} while(dd != newFace);
//}
Pierre Kraemer's avatar
Pierre Kraemer committed
210

Pierre Kraemer's avatar
Pierre Kraemer committed
211
212
void EnvMap::agentChangeFace(Agent* agent, Dart oldFace)
{
Pierre Kraemer's avatar
Pierre Kraemer committed
213
	popAgentInCells(agent, oldFace);
Pierre Kraemer's avatar
Pierre Kraemer committed
214
215
//	addCoarsenCandidate(oldFace);

Pierre Kraemer's avatar
Pierre Kraemer committed
216
	pushAgentInCells(agent, agent->part_.d);
Pierre Kraemer's avatar
Pierre Kraemer committed
217
	addRefineCandidate(agent->part_.d);
Pierre Kraemer's avatar
Pierre Kraemer committed
218
219
220
221
222
223
224
225
226
227
}

void EnvMap::updateMap()
{
	assert(map.getCurrentLevel() == map.getMaxLevel()) ;

	for(std::map<unsigned int, Dart>::iterator it = refineCandidate.begin(); it != refineCandidate.end(); ++it)
	{
		Dart d = (*it).second ;

Pierre Kraemer's avatar
Pierre Kraemer committed
228
		if(agentvect[d].size() > nbAgentsToSubdivide)
Pierre Kraemer's avatar
Pierre Kraemer committed
229
		{
Pierre Kraemer's avatar
Pierre Kraemer committed
230
231
232
			unsigned int fLevel = -1 ;
			Dart old = map.faceOldestDart(d) ;

233
			bool subdivisable = true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
234
235
236
237

			std::pair<bool,bool>& sf = subdivisableFace[d] ;
			if(sf.first == true)
				subdivisable = sf.second ;
238
			else
Pierre Kraemer's avatar
Pierre Kraemer committed
239
			{
240
241
				float maxEdgeSize = Agent::neighborDistSq_ ; // diametre de vision de l'agent au carré

Pierre Kraemer's avatar
Pierre Kraemer committed
242
				fLevel = map.faceLevel(d) ;
243
244
245
				map.setCurrentLevel(fLevel) ;
				PFP::VEC3 fCenter = Algo::Geometry::faceCentroid<PFP>(map, old, position) ;
				Dart fd = old ;
Pierre Kraemer's avatar
Pierre Kraemer committed
246
247
				do
				{
Pierre Kraemer's avatar
Pierre Kraemer committed
248
					PFP::VEC3& p = position[fd] ;
249
					PFP::VEC3 edge = Algo::Geometry::vectorOutOfDart<PFP>(map, fd, position) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
250
					PFP::VEC3 proj = fCenter - (p + (edge * (fCenter - p) / edge.norm2()) * edge) ;
251
252
					if(proj.norm2() < maxEdgeSize)
	//				if(Algo::Geometry::vectorOutOfDart<PFP>(map, fd, position).norm2() < maxEdgeSize)
Pierre Kraemer's avatar
Pierre Kraemer committed
253
					{
254
255
						subdivisable = false ;
						break ;
Pierre Kraemer's avatar
Pierre Kraemer committed
256
					}
257
258
259
260
					fd = map.phi1(fd) ;
				} while(fd != old) ;
				map.setCurrentLevel(map.getMaxLevel()) ;

Pierre Kraemer's avatar
Pierre Kraemer committed
261
262
				sf.first = true ;
				sf.second = subdivisable ;
263
264
265
266
			}

			if(subdivisable)
			{
Pierre Kraemer's avatar
Pierre Kraemer committed
267
268
269
270
				if(fLevel == -1)
					fLevel = map.faceLevel(d) ;

				sf.first = false ;
271
272
273
274
275
276

				PFP::AGENTS oldAgents(agentvect[old]);
				for(PFP::AGENTS::iterator ait = oldAgents.begin(); ait != oldAgents.end(); ++ait)
					popAgentInCells(*ait, old);

				neighborAgentvect[old].clear() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
277
278

				map.setCurrentLevel(fLevel) ;
279
280
281
282
				CellMarkerStore newF(map, FACE_CELL);
				newF.mark(old);
				Algo::IHM::subdivideFace<PFP>(map, old, position) ;

Pierre Kraemer's avatar
Pierre Kraemer committed
283
				map.setCurrentLevel(map.getMaxLevel()) ;
284
285
286
287
288
				for(PFP::AGENTS::iterator ait = oldAgents.begin(); ait != oldAgents.end(); ++ait)
				{
					resetAgentInFace(*ait) ;
					pushAgentInCells(*ait, (*ait)->part_.d) ;
				}
Pierre Kraemer's avatar
Pierre Kraemer committed
289

Pierre Kraemer's avatar
Pierre Kraemer committed
290
291
				unsigned int degree = 0 ;
				Dart dd = old ;
Pierre Kraemer's avatar
Pierre Kraemer committed
292
293
				do
				{
Pierre Kraemer's avatar
Pierre Kraemer committed
294
295
					++degree ;
					Dart d3 = dd ;
296
					do
Pierre Kraemer's avatar
Pierre Kraemer committed
297
					{
Pierre Kraemer's avatar
Pierre Kraemer committed
298
299
						Dart d4 = map.alpha1(map.alpha1(d3)) ;
						PFP::AGENTS& nad3 = neighborAgentvect[d3] ;
300
301
302
						while(d4 != d3)
						{
							if(!newF.isMarked(d4))
Pierre Kraemer's avatar
Pierre Kraemer committed
303
304
305
306
307
							{
								PFP::AGENTS& ad4 = agentvect[d4] ;
								nad3.insert(nad3.end(), ad4.begin(), ad4.end()) ;
							}
							d4 = map.alpha1(d4) ;
308
						}
Pierre Kraemer's avatar
Pierre Kraemer committed
309
310
						d3 = map.phi1(d3) ;
					} while(d3 != dd) ;
311
312

					map.setCurrentLevel(fLevel) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
313
					dd = map.phi1(dd) ;
314
315
316
317
318
					map.setCurrentLevel(map.getMaxLevel()) ;
				} while(dd != old);

				if(degree == 3)
				{
Pierre Kraemer's avatar
Pierre Kraemer committed
319
320
					Dart centerFace = map.phi2(map.phi1(old)) ;
					Dart d3 = centerFace ;
321
322
					do
					{
Pierre Kraemer's avatar
Pierre Kraemer committed
323
324
						Dart d4 = map.alpha1(map.alpha1(d3)) ;
						PFP::AGENTS& nad3 = neighborAgentvect[d3] ;
325
326
327
						while(d4 != d3)
						{
							if(!newF.isMarked(d4))
Pierre Kraemer's avatar
Pierre Kraemer committed
328
329
330
331
332
							{
								PFP::AGENTS& ad4 = agentvect[d4] ;
								nad3.insert(neighborAgentvect[d3].end(), ad4.begin(), ad4.end()) ;
							}
							d4 = map.alpha1(d4) ;
333
						}
Pierre Kraemer's avatar
Pierre Kraemer committed
334
335
						d3 = map.phi1(d3) ;
					} while(d3 != centerFace) ;
336
				}
Pierre Kraemer's avatar
Pierre Kraemer committed
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
			}
		}
	}
/*
	for(std::map<unsigned int, Dart>::iterator it = coarsenCandidate.begin(); it != coarsenCandidate.end(); ++it)
	{
		unsigned int face = (*it).first ;
		Dart d = (*it).second ;

		unsigned int fLevel = map.faceLevel(d) ;
		Dart old = map.faceOldestDart(d) ;

		if(fLevel > 0 && map.getDartLevel(old) < fLevel)
		{
			unsigned int cur = map.getCurrentLevel() ;
			map.setCurrentLevel(fLevel - 1) ;

			if(map.faceIsSubdividedOnce(old))
			{
				unsigned int degree = 0 ;
				unsigned int nbAgents = 0 ;
				Dart fit = old ;
				do
				{
Pierre Kraemer's avatar
Pierre Kraemer committed
361
					nbAgents += agentvect[fit].size() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
					++degree ;
					fit = map.phi1(fit) ;
				} while(fit != old) ;
				if(degree == 3)
				{
					map.setCurrentLevel(fLevel) ;
					Dart centerFace = map.phi2(map.phi1(old)) ;
					nbAgents += agentvect[centerFace].size() ;
					map.setCurrentLevel(fLevel - 1) ;
				}

				if(nbAgents < nbAgentsToSimplify)
				{
					PFP::AGENTS agents ;
					Dart fit = old ;
					do
					{
						map.setCurrentLevel(fLevel) ;
						unsigned int fEmb = map.getEmbedding(fit, FACE_ORBIT) ;
						if(fEmb != face)
							coarsenCandidate.erase(fEmb) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
383
384
385
386
387
						PFP::AGENTS a(agentvect[fit]) ;
						agents.insert(agents.end(), a.begin(), a.end()) ;
						map.setCurrentLevel(map.getMaxLevel()) ;
						for(PFP::AGENTS::iterator it = a.begin(); it != a.end(); ++it)
							popAgentInCells(*it, fit);
Pierre Kraemer's avatar
Pierre Kraemer committed
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
						map.setCurrentLevel(fLevel - 1) ;
						Dart nf = map.phi2(fit) ;
						if(!map.faceIsSubdivided(nf))
						{
							map.setCurrentLevel(fLevel) ;
							PFP::AGENTS& an = agentvect[nf] ;
							for(PFP::AGENTS::iterator ait = an.begin(); ait != an.end(); ++ait)
							{
								if((*ait)->part_.d == nf)
									(*ait)->part_.d = map.phi1(nf) ;
							}
							map.setCurrentLevel(fLevel - 1) ;
						}
						fit = map.phi1(fit) ;
					} while(fit != old) ;
					if(degree == 3)
					{
						map.setCurrentLevel(fLevel) ;
						Dart centerFace = map.phi2(map.phi1(old)) ;
						unsigned int fEmb = map.getEmbedding(centerFace, FACE_ORBIT) ;
						if(fEmb != face)
							coarsenCandidate.erase(fEmb) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
410
411
412
413
414
						PFP::AGENTS a(agentvect[centerFace]) ;
						agents.insert(agents.end(), a.begin(), a.end()) ;
						map.setCurrentLevel(map.getMaxLevel());
						for(PFP::AGENTS::iterator it = a.begin(); it != a.end(); ++it)
							popAgentInCells(*it, centerFace);
Pierre Kraemer's avatar
Pierre Kraemer committed
415
416
417
418
419
						map.setCurrentLevel(fLevel - 1) ;
					}

					Algo::IHM::coarsenFace<PFP>(map, old, position) ;

Pierre Kraemer's avatar
Pierre Kraemer committed
420
421
422
423
424
					// gestion des obstacles !!

					map.setCurrentLevel(map.getMaxLevel());

					agentvect[old].clear();
Pierre Kraemer's avatar
Pierre Kraemer committed
425
					for(PFP::AGENTS::iterator it = agents.begin(); it != agents.end(); ++it)
Pierre Kraemer's avatar
Pierre Kraemer committed
426
					{
Pierre Kraemer's avatar
Pierre Kraemer committed
427
						(*it)->part_.d = old ;
Pierre Kraemer's avatar
Pierre Kraemer committed
428
429
						pushAgentInCells(*it, old);
					}
Pierre Kraemer's avatar
Pierre Kraemer committed
430

Pierre Kraemer's avatar
Pierre Kraemer committed
431
432
433
434
435
436
437
438
439
440
441
					Dart dd = old;
					do
					{
						Dart ddd = map.alpha1(map.alpha1(dd));
						while(ddd != dd)
						{
							neighborAgentvect[old].insert(neighborAgentvect[old].end(), agentvect[ddd].begin(), agentvect[ddd].end());
							ddd = map.alpha1(ddd);
						}
						dd = map.phi1(dd);
					} while(dd != old);
Pierre Kraemer's avatar
Pierre Kraemer committed
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
				}
			}
			map.setCurrentLevel(cur) ;
		}
	}
*/
	map.setCurrentLevel(map.getMaxLevel()) ;
}

void EnvMap::resetAgentInFace(Agent* agent)
{
	VEC3 pos = agent->part_.m_position;
	agent->part_.m_position = Algo::Geometry::faceCentroid<PFP>(map, agent->part_.d, position);
	agent->part_.state = FACE_ORBIT;
	agent->part_.move(pos);
}