simulator.cpp 28.8 KB
Newer Older
Pierre Kraemer's avatar
Pierre Kraemer committed
1
#include "simulator.h"
2
#include <cstdlib>
Pierre Kraemer's avatar
Pierre Kraemer committed
3

David Cazier's avatar
David Cazier committed
4
5
6
7
8
9
10
11
12
Simulator::Simulator(int minSize) :
	timeStep_(0.2f),
	globalTime_(0.0f),
	nbSteps_(0),
	nbUpdates(0),
	nbSorts(0),
	nbRefineCandidate(0),
	nbCoarsenCandidate(0),
	nearNeighbors(0),
David Cazier's avatar
David Cazier committed
13
	totalNeighbors(0),
pitiot's avatar
pitiot committed
14
15
	avoidance(0),
	nb_dead(0)
Pierre Kraemer's avatar
Pierre Kraemer committed
16
{
17
18
	srand(10) ;
	nbStepsPerUnit_ = 1 / timeStep_ ;
David Cazier's avatar
David Cazier committed
19
	init(0, minSize, 2.0f) ;
Thomas's avatar
Thomas committed
20
21
22
23
}

Simulator::~Simulator()
{
David Cazier's avatar
David Cazier committed
24
	for (unsigned int i = 0; i < agents_.size(); ++i)
David Cazier's avatar
David Cazier committed
25
		delete agents_[i] ;
Thomas's avatar
Thomas committed
26
27
}

David Cazier's avatar
David Cazier committed
28
void Simulator::init(unsigned int config, int minSize, float dimension, bool enablePathFinding)
Thomas's avatar
Thomas committed
29
{
30
	std::cout << "Setup scenario" << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
31

David Cazier's avatar
David Cazier committed
32
	envMap_.init(config, 2500.0f, 2000.0f, minSize, 200.0f) ;
Thomas's avatar
Thomas committed
33

David Cazier's avatar
David Cazier committed
34
35
	switch (config)
	{
David Cazier's avatar
David Cazier committed
36
37
38
39
		case 0 :
			setupCircleScenario(1000) ;
			break ;
		case 1 :
David Cazier's avatar
David Cazier committed
40
			setupCorridorScenario(500) ;
David Cazier's avatar
David Cazier committed
41
42
43
44
45
46
			break ;
		case 2 :
			setupScenario(1000) ;
			break ;
		case 3 :
			setupCityScenario(20, 20) ;
David Cazier's avatar
David Cazier committed
47
48
//			setupCityScenario(-1.0f * (12 * (70.0f / 2.0f) - 10),
//					-1.0f * (12 * (70.0f / 2.0f) - 10), 20, 20);
David Cazier's avatar
David Cazier committed
49
50
51
52
			break ;
		case 4 :
			importAgents("myAgents.pos") ;
			break ;
Thomas's avatar
Thomas committed
53
54
	}

Pierre Kraemer's avatar
Pierre Kraemer committed
55
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
56
57
58
	if (enablePathFinding)
	{
		if (dimension == 2.0f)
David Cazier's avatar
David Cazier committed
59
		addPathsToAgents() ;
60
		else if (dimension == 2.5f) addPathsToAgents_height() ;
Thomas's avatar
Thomas committed
61
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
62
#endif
63

David Cazier's avatar
David Cazier committed
64
//	setupMovingObstacle(40);
65

David Cazier's avatar
David Cazier committed
66
	unsigned nbAgents = agents_.size() ;
67

68
69
//	addPathToCorner();

David Cazier's avatar
David Cazier committed
70
71
	for (unsigned int i = 0; i < nbAgents; ++i)
	{
David Cazier's avatar
David Cazier committed
72
73
		agents_[i]->updateObstacleNeighbors() ;
		agents_[i]->updateAgentNeighbors() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
74
	}
75

Pierre Kraemer's avatar
Pierre Kraemer committed
76
#ifndef SPATIAL_HASHING
77
	envMap_.subdivideToProperLevel() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
78
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
79
80
}

81
void Simulator::setupCircleScenario(unsigned int nbMaxAgent)
Pierre Kraemer's avatar
Pierre Kraemer committed
82
{
David Cazier's avatar
David Cazier committed
83
	std::cout << " - Setup Circle Scenario : " << nbMaxAgent << " agents" << std::endl ;
84
85
	float pi = 3.14159265358979323846f ;
	float r = 800.0f ;
David Cazier's avatar
David Cazier committed
86

pitiot's avatar
pitiot committed
87
88
89
90
91
92
93
	for (unsigned int i = 0; i < movingObstacles_.size(); ++i)
	{
		movingObstacles_[i]->updateFixedObstNeighbors();
		movingObstacles_[i]->computePrefVelocity();
		movingObstacles_[i]->update();
	}

Pierre Kraemer's avatar
Pierre Kraemer committed
94
#ifndef SPATIAL_HASHING
Pierre Kraemer's avatar
Pierre Kraemer committed
95
	envMap_.clearUpdateCandidates();
Pierre Kraemer's avatar
Pierre Kraemer committed
96
	envMap_.map.setCurrentLevel(0);
Pierre Kraemer's avatar
Pierre Kraemer committed
97
#endif
98

David Cazier's avatar
David Cazier committed
99
100
	// TODO utiliser agents_.size() ?
	for (unsigned int i = 0; i < nbMaxAgent; ++i)
David Cazier's avatar
David Cazier committed
101
	{
102
		VEC3 posagent(std::cos(i * 2.0f * pi / float(nbMaxAgent)) * r,
David Cazier's avatar
David Cazier committed
103
		              std::sin(i * 2.0f * pi / float(nbMaxAgent)) * r, 0) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
104

Pierre Kraemer's avatar
Pierre Kraemer committed
105
#ifdef SPATIAL_HASHING
106
107
		Agent* a = new Agent(this, posagent) ;
		agents_.push_back(a) ;
David Cazier's avatar
David Cazier committed
108
		envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
109
#else
110
111
		Dart dCell = envMap_.getBelongingCell(posagent) ;
		agents_.push_back(new Agent(this, posagent, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
112

David Cazier's avatar
David Cazier committed
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
		// TODO Code Thomas à vérifier
//	envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel());
//	nb_dead=0;
//	for (unsigned int i = 0; i < agents_.size(); ++i)
//	{
//		if (agents_[i]->alive)
//			{
//			Dart oldFace = agents_[i]->part_.d;
//
//		agents_[i]->update();
//
//		if(agents_[i]->part_.crossCell != CGoGN::Algo::MovingObjects::NO_CROSS)
//		{
//				envMap_.agentChangeFace(agents_[i], oldFace);
//		}
//			}
//		else nb_dead++;
//
//	}
Pierre Kraemer's avatar
Pierre Kraemer committed
132
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
133

David Cazier's avatar
David Cazier committed
134
		agents_.back()->goals_.push_back(-1.0f * posagent) ;
135
136
		agents_.back()->curGoal_ = 0 ;
	}
Thomas's avatar
Thomas committed
137

138
	std::cout << "nb agents : " << agents_.size() << std::endl ;
Thomas's avatar
Thomas committed
139

140
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
141
142
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d) ;
143
#endif
Thomas's avatar
Thomas committed
144
145
}

146
void Simulator::setupCorridorScenario(unsigned int nbMaxAgent)
Thomas's avatar
Thomas committed
147
{
David Cazier's avatar
David Cazier committed
148
	std::cout << " - Setup Corridor Scenario : " << nbMaxAgent << " agents" << std::endl ;
David Cazier's avatar
David Cazier committed
149
150
151
152
153
154
	int xBorder = 530 ;
	int variationX = 250 ;
	int left = envMap_.geometry.min()[0] + xBorder ;
	int right = envMap_.geometry.max()[0] - xBorder - variationX ;
	int variationY = envMap_.geometry.size(1) * 0.4f ;
	int middle = -variationY / 2 ;
155

David Cazier's avatar
David Cazier committed
156
//	float r = 220.0f;
David Cazier's avatar
David Cazier committed
157
158
	for (unsigned int i = 0; i < nbMaxAgent; ++i)
	{
David Cazier's avatar
David Cazier committed
159
160
		VEC3 posagent(left + rand() % variationX, middle + rand() % variationY, 0) ;
		VEC3 posgoal(right + rand() % variationX, middle + rand() % variationY, 0) ;
161
162
		VEC3 tmp ;

David Cazier's avatar
David Cazier committed
163
164
		if (i % 2 == 1)
		{
165
166
167
168
			tmp = posgoal ;
			posgoal = posagent ;
			posagent = tmp ;
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
169
170
171
172

#ifdef SPATIAL_HASHING
		Agent* a = new Agent(this, posagent) ;
		agents_.push_back(a) ;
David Cazier's avatar
David Cazier committed
173
		envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
174
#else
175
176
		Dart dCell = envMap_.getBelongingCell(posagent) ;
		agents_.push_back(new Agent(this, posagent, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
177
178
#endif

179
180
		agents_.back()->goals_.push_back(posgoal) ;
		agents_.back()->curGoal_ = 0 ;
Thomas's avatar
Thomas committed
181
182
	}

David Cazier's avatar
David Cazier committed
183
	std::cout << "nb agents : " << agents_.size() << std::endl ;
Thomas's avatar
Thomas committed
184

Pierre Kraemer's avatar
Pierre Kraemer committed
185
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
186
187
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
188
#endif
Thomas's avatar
Thomas committed
189
190
}

David Cazier's avatar
David Cazier committed
191
void Simulator::setupCityScenario(int nbLines, int nbRank)
Thomas's avatar
Thomas committed
192
{
David Cazier's avatar
David Cazier committed
193
194
195
196
197
	std::cout << " - Setup City Scenario : " << nbLines << " x " << nbRank << std::endl ;
	int xBorder = 300 ;
	int yBorder = 200 ;
	int xCornerDown = envMap_.geometry.min()[0] + xBorder ;
	int yCornerDown = envMap_.geometry.min()[1] + yBorder ;
Thomas's avatar
Thomas committed
198

David Cazier's avatar
David Cazier committed
199
	VEC3 posagent(xCornerDown - xBorder/2, yCornerDown - yBorder/2, 0.0f) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
200
201
202
#ifdef SPATIAL_HASHING
	Agent* a = new Agent(this, posagent) ;
	agents_.push_back(a) ;
David Cazier's avatar
David Cazier committed
203
	envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
204
#else
205
206
	Dart dCell = envMap_.getBelongingCell(posagent) ;
	agents_.push_back(new Agent(this, posagent, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
207
#endif
David Cazier's avatar
David Cazier committed
208
	agents_.back()->goals_.push_back(-1.0f * posagent) ;
209
	agents_.back()->curGoal_ = 0 ;
210

David Cazier's avatar
David Cazier committed
211
212
213
214
	for (int i = 0; i < nbLines; ++i)
	{
		for (int j = 0; j < nbRank; ++j)
		{
215
			VEC3 posagent(xCornerDown + i * 10.0f, yCornerDown + j * 10.0f, 0.0f) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
216
217
218
#ifdef SPATIAL_HASHING
			Agent* a = new Agent(this, posagent) ;
			agents_.push_back(a) ;
David Cazier's avatar
David Cazier committed
219
			envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
220
#else
221
222
			Dart dCell = envMap_.getBelongingCell(posagent) ;
			agents_.push_back(new Agent(this, posagent, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
223
#endif
David Cazier's avatar
David Cazier committed
224
			agents_.back()->goals_.push_back(-1.0f * posagent) ;
225
			agents_.back()->curGoal_ = 0 ;
Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
226
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
227
	}
Thomas's avatar
Thomas committed
228

David Cazier's avatar
David Cazier committed
229
	std::cout << "nb agents : " << agents_.size() << std::endl ;
Thomas's avatar
Thomas committed
230

Pierre Kraemer's avatar
Pierre Kraemer committed
231
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
232
233
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
234
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
235
236
}

237
void Simulator::setupScenario(unsigned int nbMaxAgent)
Pierre Kraemer's avatar
Pierre Kraemer committed
238
{
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
//	Geom::BoundingBox<PFP::VEC3> bb(envMap_.position.begin());
//	for(unsigned int v = envMap_.position.begin() ; v != envMap_.position.end() ; envMap_.position.next(v))
//		bb.addPoint(envMap_.position[v]);
//	VEC3 posinit = bb.center();
//	posinit[1] = bb.min()[1] + 35.0f;

//	float ecart = bb.size(0)/nbMaxAgent;
//	posinit[0] -= ecart * (nbMaxAgent/2 - 0.5f);
//	for(unsigned int i = 0; i < nbMaxAgent; ++i)
//	{
//		VEC3 posagent = posinit + VEC3(ecart * i, 0.0f, 0.0f);
//		agents_.push_back(new Agent(this, posagent, envMap_.getBelongingCell(posagent)));
//		agents_.back()->goals_.push_back(posagent);
//		VEC3 goal = posagent;
//		goal *= -1.0f;
//		agents_.back()->goals_.push_back(goal);
//		agents_.back()->curGoal_ = 1;
//		agents_.back()->finalGoal = goal;
//		agents_.back()->finalDart = envMap_.getBelongingCell(goal);
//	}

Pierre Kraemer's avatar
Pierre Kraemer committed
260
	/*
261
<<<<<<< HEAD
David Cazier's avatar
David Cazier committed
262
263
264
265
	 * Add agents, specifying their start position, and store their goals on the
	 * opposite side of the environment.
	 */
	Dart d = envMap_.map.begin() ;
David Cazier's avatar
David Cazier committed
266
	CellMarker<FACE> filled(envMap_.map) ;
David Cazier's avatar
David Cazier committed
267
268
269
270
271
272

	unsigned int nbx = 5 ;
	unsigned int nby = 5 ;

	unsigned int bMax = nbx * nby > 0 ? nbMaxAgent / (nbx * nby) : nbMaxAgent ;

David Cazier's avatar
David Cazier committed
273
274
	for (unsigned int i = 0; i < bMax && d != envMap_.map.end(); ++i)
	{
David Cazier's avatar
David Cazier committed
275
276
277
		bool found = false ;
		VEC3 pos ;
		Dart dCell ;
David Cazier's avatar
David Cazier committed
278
279
280
281
282
		while (!found && d != envMap_.map.end())
		{
			if (!filled.isMarked(d)
			    && !envMap_.buildingMark.isMarked(d) /*&& envMap_.pedWayMark.isMarked(d)*/)
			{
David Cazier's avatar
David Cazier committed
283
				filled.mark(d) ;
284
				pos = Algo::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
David Cazier's avatar
David Cazier committed
285
286
287
				pos[2] = 0 ;
				dCell = d ;
				found = true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
288
			}
David Cazier's avatar
David Cazier committed
289
			envMap_.map.next(d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
290
291
		}

David Cazier's avatar
David Cazier committed
292
293
		if (found)
		{
294
295
			float ecart = 3.0f ;
			VEC3 posinit = VEC3(pos[0] - (float(nbx) / 2.0f * ecart),
David Cazier's avatar
David Cazier committed
296
297
298
299
300
			                    pos[1] - (float(nby) / 2.0f * ecart), pos[2]) ;
			for (unsigned int curx = 0; curx < nbx; ++curx)
			{
				for (unsigned int cury = 0; cury < nby; ++cury)
				{
301
					VEC3 posagent = posinit + VEC3(ecart * curx, ecart * cury, 0.0f) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
302
303
304
#ifdef SPATIAL_HASHING
					Agent* a = new Agent(this, posagent) ;
					agents_.push_back(a) ;
David Cazier's avatar
David Cazier committed
305
					envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
306
#else
307
					agents_.push_back(new Agent(this, posagent, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
308
#endif
309
					agents_.back()->goals_.push_back(posagent) ;
David Cazier's avatar
David Cazier committed
310
					agents_.back()->goals_.push_back(-1.0f * posagent) ;
David Cazier's avatar
David Cazier committed
311
312
313
314
315
316
317
318
319
320
321

// TODO Code Thomas à vérifier
//					VEC3 oppos = -1.0f * posagent;
//					Dart d1 =envMap_.getBelongingCell(oppos);
//
//					if (!envMap_.buildingMark.isMarked(d1))
//						agents_.back()->goals_.push_back(oppos);
//					else if (oppos[0]+50 <envMap_.max_for_obstacles && oppos[1]+50 <envMap_.max_for_obstacles)
//						agents_.back()->goals_.push_back(oppos + VEC3(50,50,0));
//					else
//						agents_.back()->goals_.push_back(oppos + VEC3(-50,-50,0));
322
					agents_.back()->curGoal_ = 1 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
323
324
325
326
				}
			}
		}
	}
David Cazier's avatar
David Cazier committed
327
328
	std::cout << "nb agents : " << agents_.size() << std::endl ;
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
329

Pierre Kraemer's avatar
Pierre Kraemer committed
330
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
331
332
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d) ;
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
#endif
}

void Simulator::doStep()
{
#ifndef SPATIAL_HASHING
	envMap_.clearUpdateCandidates() ;
	envMap_.map.setCurrentLevel(0) ;
#endif

//	for (unsigned int i = 0; i < movingObstacles_.size(); ++i)
//	{
//		movingObstacles_[i]->updateAgentNeighbors();
//		movingObstacles_[i]->computePrefVelocity();
//		movingObstacles_[i]->computeNewVelocity();
//
//		movingObstacles_[i]->update();
//	}

David Cazier's avatar
David Cazier committed
352
353
	for (unsigned int i = 0; i < agents_.size(); ++i)
	{
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
		agents_[i]->updateObstacleNeighbors() ;

#ifndef SPATIAL_HASHING
		envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
#endif
		agents_[i]->updateAgentNeighbors() ;
#ifndef SPATIAL_HASHING
		envMap_.map.setCurrentLevel(0) ;
#endif

		agents_[i]->computePrefVelocity() ;
		agents_[i]->computeNewVelocity() ;
	}

//	boost::thread thread1(&ThreadUpdateInfo::run, tc1);
//	boost::thread thread2(&ThreadUpdateInfo::run, tc2);
//	boost::thread thread3(&ThreadUpdateInfo::run, tc3);
//	boost::thread thread4(&ThreadUpdateInfo::run, tc4);
//
//	thread1.join();
//	thread2.join();
//	thread3.join();
//	thread4.join();

#ifdef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
379
	for (unsigned int i = 0; i < agents_.size(); ++i)
380
	{
David Cazier's avatar
David Cazier committed
381
		Geom::Vec2ui cold = envMap_.agentPositionCell(agents_[i]) ;
382
		agents_[i]->update() ;
David Cazier's avatar
David Cazier committed
383
384
		Geom::Vec2ui cnew = envMap_.agentPositionCell(agents_[i]) ;
		if (cnew != cold)
385
		{
David Cazier's avatar
David Cazier committed
386
387
			envMap_.removeAgentFromGrid(agents_[i], cold) ;
			envMap_.addAgentInGrid(agents_[i], cnew) ;
388
389
390
391
392
		}
	}
#else
	envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;

David Cazier's avatar
David Cazier committed
393
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
394
	{
395
396
397
398
399
400
		Dart oldFace = agents_[i]->part_.d ;
		agents_[i]->update() ;

//		if(envMap_.map.getEmbedding(oldFace, FACE) != envMap_.map.getEmbedding(agents_[i]->part_.d, FACE))
		if (agents_[i]->part_.crossCell != CGoGN::Algo::MovingObjects::NO_CROSS)
//		switch(agents_[i]->part_.crossCell)
David Cazier's avatar
David Cazier committed
401
		{
402
403
404
405
406
407
408
409
410
411
412
413
414
//			case CGoGN::Algo::MovingObjects::CROSS_EDGE :
//				envMap_.agentChangeFaceThroughEdge(agents_[i]);
//				break;
//			case CGoGN::Algo::MovingObjects::CROSS_OTHER :
			envMap_.agentChangeFace(agents_[i], oldFace) ;
//				break;
		}

	}

	nbRefineCandidate += envMap_.refineCandidate.size() ;
	nbCoarsenCandidate += envMap_.coarsenCandidate.size() ;
	envMap_.updateMap() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
415
#endif
416
417
418
419
420
421
422
423
424
425
426
427
428
429

	globalTime_ += timeStep_ ;
	++nbSteps_ ;

//	if(nbSteps % 2000 == 0)
//	{
//		std::cout << "swap goals (globalTime -> " << globalTime_ << ")" << std::endl ;
//		swapAgentsGoals();
//	}
}

bool Simulator::reachedGoal()
{
	/* Check if all agents have reached their goals. */
David Cazier's avatar
David Cazier committed
430
	for (unsigned int i = 0; i < agents_.size(); ++i)
431
		if ((agents_[i]->getPosition() - agents_[i]->goals_[agents_[i]->curGoal_]).norm2()
David Cazier's avatar
David Cazier committed
432
		    > agents_[i]->radius_ * agents_[i]->radius_) return false ;
433
434

	return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
435
}
436

Pierre Kraemer's avatar
Pierre Kraemer committed
437
#ifndef SPATIAL_HASHING
438
439
void Simulator::addPathToCorner()
{
David Cazier's avatar
David Cazier committed
440
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
441
	{
David Cazier's avatar
David Cazier committed
442
443
		agents_[i]->goals_.clear() ;
		agents_.back()->curGoal_ = 1 ;
444

David Cazier's avatar
David Cazier committed
445
446
		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = agents_[i]->finalDart ;
447

448
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
David Cazier's avatar
David Cazier committed
449
450
451
			envMap_.position, dStart,
			dStop,
			envMap_.buildingMark) ;
452

David Cazier's avatar
David Cazier committed
453
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
454
		{
455
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
456
457
458
459
460

//			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)];
//			dest /= 2.0f;
//			dest[2]=0;

David Cazier's avatar
David Cazier committed
461
			agents_[i]->goals_.push_back(dest) ;
462
463
464
465
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
466
467
void Simulator::addPathsToAgents()
{
Thomas's avatar
Thomas committed
468
	//city
David Cazier's avatar
David Cazier committed
469
	unsigned int dartDistForPath = 50 ;
David Cazier's avatar
David Cazier committed
470
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
471
	{
David Cazier's avatar
David Cazier committed
472
473
474
475
		agents_[i]->goals_.clear() ;

		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
David Cazier's avatar
David Cazier committed
476
477
478
		for (unsigned int j = 0 ; /*!envMap_.pedWayMark.isMarked(dStop) ||*/
			envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath + rand() * 20
			|| envMap_.map.sameFace(dStop, dStart) ; ++j)
David Cazier's avatar
David Cazier committed
479
		{
David Cazier's avatar
David Cazier committed
480
481
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end()) dStop = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
482
		}
483

484
485
//		std::cout << "dest1" << Algo::Geometry::faceCentroid<PFP>(envMap_.map, dStop, envMap_.position) << std::endl;

486
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
David Cazier's avatar
David Cazier committed
487
488
489
			envMap_.position, dStart,
			dStop,
			envMap_.buildingMark) ;
490

David Cazier's avatar
David Cazier committed
491
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
492
		{
493
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
494
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
495
496
			dest /= 2.0f ;
			dest[2] = 0 ;
497

David Cazier's avatar
David Cazier committed
498
			agents_[i]->goals_.push_back(dest) ;
499
		}
500

David Cazier's avatar
David Cazier committed
501
		Dart dStop2 = dStop ;
David Cazier's avatar
David Cazier committed
502
503
504
		for (unsigned int j = 0 ; /*!envMap_.pedWayMark.isMarked(dStop) ||*/
			envMap_.buildingMark.isMarked(dStop2) || j < dartDistForPath + rand() * 20
			|| envMap_.map.sameFace(dStop, dStop2) || envMap_.map.sameFace(dStop2, dStart) ; ++j)
David Cazier's avatar
David Cazier committed
505
		{
David Cazier's avatar
David Cazier committed
506
507
			envMap_.map.next(dStop2) ;
			if (dStop2 == envMap_.map.end()) dStop2 = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
508
		}
509

510
511
//		std::cout << "dest2" << Algo::Geometry::faceCentroid<PFP>(envMap_.map, dStop2, envMap_.position) << std::endl;

David Cazier's avatar
David Cazier committed
512
		path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map, envMap_.position, dStop, dStop2,
David Cazier's avatar
David Cazier committed
513
			envMap_.buildingMark) ;
514

David Cazier's avatar
David Cazier committed
515
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
516
		{
517
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
518
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
519
520
			dest /= 2.0f ;
			dest[2] = 0 ;
521

David Cazier's avatar
David Cazier committed
522
			agents_[i]->goals_.push_back(dest) ;
523
		}
524

David Cazier's avatar
David Cazier committed
525
		path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map, envMap_.position, dStop2, dStart,
David Cazier's avatar
David Cazier committed
526
			envMap_.buildingMark) ;
527

528
529
//		std::cout << "destStart" << Algo::Geometry::faceCentroid<PFP>(envMap_.map, dStart, envMap_.position) << std::endl;

David Cazier's avatar
David Cazier committed
530
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
531
		{
532
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
533
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
534
535
			dest /= 2.0f ;
			dest[2] = 0 ;
536

David Cazier's avatar
David Cazier committed
537
			agents_[i]->goals_.push_back(dest) ;
538
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
539
540
	}
}
541

Thomas's avatar
Thomas committed
542
543
544
void Simulator::addPathsToAgents_height()
{
	//city
David Cazier's avatar
David Cazier committed
545
	unsigned int dartDistForPath = 50 ;
David Cazier's avatar
David Cazier committed
546
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
547
	{
David Cazier's avatar
David Cazier committed
548
549
550
551
		agents_[i]->goals_.clear() ;

		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
David Cazier's avatar
David Cazier committed
552
553
554
		for (unsigned int j = 0 ;
			envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath + rand() * 20
			|| envMap_.map.sameFace(dStop, dStart) ; ++j)
David Cazier's avatar
David Cazier committed
555
		{
David Cazier's avatar
David Cazier committed
556
557
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end()) dStop = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
558
559
		}

560
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
David Cazier's avatar
David Cazier committed
561
562
563
			envMap_.position, dStart,
			dStop,
			envMap_.buildingMark) ;
Thomas's avatar
Thomas committed
564

David Cazier's avatar
David Cazier committed
565
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
566
		{
567
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
David Cazier's avatar
David Cazier committed
568
			agents_[i]->goals_.push_back(dest) ;
Thomas's avatar
Thomas committed
569
570
		}

David Cazier's avatar
David Cazier committed
571
		Dart dStop2 = dStop ;
David Cazier's avatar
David Cazier committed
572
573
574
		for (unsigned int j = 0 ;
			envMap_.buildingMark.isMarked(dStop2) || j < dartDistForPath + rand() * 20
			|| envMap_.map.sameFace(dStop, dStop2) || envMap_.map.sameFace(dStop2, dStart) ; ++j)
David Cazier's avatar
David Cazier committed
575
		{
David Cazier's avatar
David Cazier committed
576
577
			envMap_.map.next(dStop2) ;
			if (dStop2 == envMap_.map.end()) dStop2 = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
578
579
		}

David Cazier's avatar
David Cazier committed
580
		path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map, envMap_.position, dStop, dStop2,
David Cazier's avatar
David Cazier committed
581
			envMap_.buildingMark) ;
Thomas's avatar
Thomas committed
582

David Cazier's avatar
David Cazier committed
583
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
584
		{
585
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
David Cazier's avatar
David Cazier committed
586
			agents_[i]->goals_.push_back(dest) ;
Thomas's avatar
Thomas committed
587
588
		}

David Cazier's avatar
David Cazier committed
589
		path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map, envMap_.position, dStop2, dStart,
David Cazier's avatar
David Cazier committed
590
			envMap_.buildingMark) ;
Thomas's avatar
Thomas committed
591

David Cazier's avatar
David Cazier committed
592
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
593
		{
594
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
David Cazier's avatar
David Cazier committed
595
			agents_[i]->goals_.push_back(dest) ;
Thomas's avatar
Thomas committed
596
597
598
		}
	}
}
Pierre Kraemer's avatar
Pierre Kraemer committed
599
#endif
Thomas's avatar
Thomas committed
600

Pierre Kraemer's avatar
Pierre Kraemer committed
601
602
bool Simulator::importAgents(std::string filename)
{
David Cazier's avatar
David Cazier committed
603
	std::ifstream myfile(filename.c_str(), std::ios::in) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
604

David Cazier's avatar
David Cazier committed
605
606
	if (!myfile.good())
	{
David Cazier's avatar
David Cazier committed
607
608
		std::cerr << "(export) Unable to open file " << filename << std::endl ;
		return false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
609
610
	}

David Cazier's avatar
David Cazier committed
611
612
613
	std::string line, token ;
	while (myfile.good())
	{
David Cazier's avatar
David Cazier committed
614
		getline(myfile, line) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
615

David Cazier's avatar
David Cazier committed
616
617
		if (line.size() > 1)
		{
David Cazier's avatar
David Cazier committed
618
			std::istringstream iss(line) ;
David Cazier's avatar
David Cazier committed
619
			float x, y, z ;
Pierre Kraemer's avatar
Pierre Kraemer committed
620

David Cazier's avatar
David Cazier committed
621
622
623
			iss >> x ;
			iss >> y ;
			iss >> z ;
Pierre Kraemer's avatar
Pierre Kraemer committed
624

625
			VEC3 pos(x, y, z) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
626
627
628
#ifdef SPATIAL_HASHING
			Agent* a = new Agent(this, pos) ;
			agents_.push_back(a) ;
David Cazier's avatar
David Cazier committed
629
			envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
630
#else
631
632
			Dart dCell = envMap_.getBelongingCell(pos) ;
			agents_.push_back(new Agent(this, pos, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
633
#endif
634
			agents_.back()->goals_.push_back(pos) ;
David Cazier's avatar
David Cazier committed
635
			agents_.back()->goals_.push_back(-1.0f * pos) ;
636
			agents_.back()->curGoal_ = 1 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
637
638
639
		}
	}

David Cazier's avatar
David Cazier committed
640
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
641

Pierre Kraemer's avatar
Pierre Kraemer committed
642
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
643
644
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
645
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
646

David Cazier's avatar
David Cazier committed
647
648
	myfile.close() ;
	return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
649
650
651
652
}

bool Simulator::exportAgents(std::string filename)
{
David Cazier's avatar
David Cazier committed
653
	std::ofstream out(filename.c_str(), std::ios::out) ;
David Cazier's avatar
David Cazier committed
654
655
	if (!out.good())
	{
David Cazier's avatar
David Cazier committed
656
657
		std::cerr << "(export) Unable to open file " << filename << std::endl ;
		return false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
658
659
	}

David Cazier's avatar
David Cazier committed
660
	for (unsigned int i = 0; i < agents_.size(); ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
661
#ifdef SPATIAL_HASHING
662
		out << agents_[i]->pos << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
663
#else
David Cazier's avatar
David Cazier committed
664
	out << agents_[i]->part_.getPosition() << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
665
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
666

David Cazier's avatar
David Cazier committed
667
668
	out.close() ;
	return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
669
670
671
672
}

void Simulator::swapAgentsGoals()
{
David Cazier's avatar
David Cazier committed
673
	unsigned int nbAgents = agents_.size() ;
David Cazier's avatar
David Cazier committed
674
675
	for (unsigned int i = 0; i < nbAgents; ++i)
	{
David Cazier's avatar
David Cazier committed
676
		unsigned int r = rand() % nbAgents ;
Thomas's avatar
Thomas committed
677
678
679
680
681
682
683
//		for(unsigned int nbg = 0; nbg < agents_[i]->goals_.size(); ++nbg)
//		{
//			VEC3 g = agents_[i]->goals_[nbg];
//			agents_[i]->goals_[nbg] = agents_[r]->goals_[nbg];
//			agents_[r]->goals_[nbg] = g;
//		}

684
		std::swap(agents_[i]->goals_, agents_[r]->goals_) ;
Thomas's avatar
Thomas committed
685
686
	}
}
Pierre Kraemer's avatar
Pierre Kraemer committed
687

Thomas's avatar
Thomas committed
688
689
Geom::BoundingBox<VEC3> Simulator::getAgentsBB()
{
David Cazier's avatar
David Cazier committed
690
691
	Geom::BoundingBox<VEC3> bb ;
	unsigned int nbAgents = agents_.size() ;
David Cazier's avatar
David Cazier committed
692
693
	for (unsigned int i = 0; i < nbAgents; ++i)
	{
David Cazier's avatar
David Cazier committed
694
		bb.addPoint(agents_[i]->getPosition()) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
695
	}
Thomas's avatar
Thomas committed
696

David Cazier's avatar
David Cazier committed
697
	return bb ;
Pierre Kraemer's avatar
Pierre Kraemer committed
698
}
699

pitiot's avatar
pitiot committed
700
701
702
703
704
705
706
void Simulator::setupMovingObstacle( unsigned int nb_obst)
{
	float xSize = 10.0f;
	float ySize = 45.0f;
	VEC3 goal (50.0f,50.0f,0);
	VEC3 posInit(0,0,0);
	VEC3 possiblepos(0,0,0);
707
708
709
710
	//	//posInit[0] -= 200.0f;
	//	posInit[1] -= 100.0f;
	//
	//
pitiot's avatar
pitiot committed
711
	std::vector<VEC3> vPos;
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
	//	vPos.push_back(posInit);
	//	posInit[0] -= xSize;
	////	vPos.push_back(posInit);
	//	posInit[1] -= ySize;
	//	vPos.push_back(posInit);
	//	posInit[0] += 2*xSize;
	////	posInit[1] += ySize;
	//	vPos.push_back(posInit);
	////	posInit[0] -= xSize;
	////	posInit[1] += ySize;
	////		vPos.push_back(posInit);
	//
	//
	////	CGoGNout << "ligne "<< i << vPos[i] << CGoGNendl;
	//
	//	MovingObstacle* mo = new MovingObstacle(1,&envMap_, vPos,goal,0);
	//	movingObstacles_.push_back(mo);
	//
	//
	//		goal =PFP::VEC3(-50.0f,50.0f,0);
	//		posInit=PFP::VEC3(0,0,0);
	//		//posInit[0] -= 200.0f;
	//		posInit[1] += 100.0f;
	//
	//
	//		vPos.clear();
	//		vPos.push_back(posInit);
	//		posInit[0] -= xSize;
	//	//	vPos.push_back(posInit);
	//		posInit[1] -= ySize;
	//		vPos.push_back(posInit);
	//		posInit[0] += 2*xSize;
	//	//	posInit[1] += ySize;
	//		vPos.push_back(posInit);
	//	//	posInit[0] -= xSize;
	//	//	posInit[1] += ySize;
	//	//		vPos.push_back(posInit);
	//
	//
	//	//	CGoGNout << "ligne "<< i << vPos[i] << CGoGNendl;
	//
	//		MovingObstacle* mo2 = new MovingObstacle(2,&envMap_, vPos,goal,0);
	//		movingObstacles_.push_back(mo2);

	MovingObstacle* mo4;
	float xx, yy;
	float sx, sy;

	for (unsigned int i =0; i <(nb_obst); i++)
	{
		sx=rand() % 2 +1;
		sy=rand() % 2 +1;
		xx=rand() % (int)envMap_.max_for_obstacles;
		if(sx-1==1)xx*=-1;
		yy=rand() % (int)envMap_.max_for_obstacles;
		if(sy-1==1)yy*=-1;
		VEC3 possiblepos(xx,yy,0);
		Dart d1 =envMap_.getBelongingCell(possiblepos);
		if (envMap_.buildingMark.isMarked(d1))
		{
			if (((possiblepos[0]+50) <envMap_.max_for_obstacles) && ((possiblepos[1]+50) <envMap_.max_for_obstacles)) possiblepos += VEC3(50,50,0);
			else possiblepos += VEC3(-50,-50,0);
		}
		xx=possiblepos[0];
		yy=possiblepos[1];

		goal =PFP::VEC3(xx,-yy,0);

		posInit=PFP::VEC3(0,0,0);
		//posInit[0] -= 200.0f;
		posInit[1] += yy;
		posInit[0] += xx;
		possiblepos=posInit;
		d1 =envMap_.getBelongingCell(possiblepos);
David Cazier's avatar
David Cazier committed
786
787

		// TODO Check sideSide
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
1001
1002
1003
1004
1005
1006
1007
1008
1009
1010
1011
1012
1013
1014
1015
1016
1017
1018
1019
1020
1021
1022
1023
1024
1025
1026
1027
1028
1029
1030
		if (envMap_.buildingMark.isMarked(d1))
		{
			if (((possiblepos[0]+envMap_.sideSize) <envMap_.max_for_obstacles) && ((possiblepos[1]+envMap_.sideSize) <envMap_.max_for_obstacles)) possiblepos += VEC3(envMap_.sideSize,envMap_.sideSize,0);
			else if (((possiblepos[0]-envMap_.sideSize) <envMap_.max_for_obstacles) && ((possiblepos[1]-envMap_.sideSize) <envMap_.max_for_obstacles)) possiblepos += VEC3(-envMap_.sideSize,-envMap_.sideSize,0);
			else if (((possiblepos[0]-envMap_.sideSize) <envMap_.max_for_obstacles) && ((possiblepos[1]+envMap_.sideSize) <envMap_.max_for_obstacles)) possiblepos += VEC3(-envMap_.sideSize,envMap_.sideSize,0);
			else possiblepos += VEC3(envMap_.sideSize,-envMap_.sideSize,0);
		}
		posInit=possiblepos;
		vPos.clear();
		vPos.push_back(posInit);
		//								posInit[0] -= xSize/2;
		//								vPos.push_back(posInit);
		posInit[0] -= xSize;
		posInit[1] -= ySize/2;
		vPos.push_back(posInit);
		posInit[0] += xSize;
		posInit[1] -= ySize/2;
		vPos.push_back(posInit);
		//								posInit[0] += xSize;
		//	posInit[1] += ySize;
		//								vPos.push_back(posInit);
		posInit[0] += xSize;
		posInit[1] += ySize/2;
		vPos.push_back(posInit);
		//								posInit[0] -= xSize;
		//								posInit[1] += ySize/2;
		//								vPos.push_back(posInit);


		//	CGoGNout << "ligne "<< i << vPos[i] << CGoGNendl;

		mo4= new MovingObstacle(i,&envMap_, vPos,goal,0);
		movingObstacles_.push_back(mo4);
	}
	//				xSize = 15.0f;
	//				ySize = 45.0f;
	//				for (unsigned int i =0; i <(2*nb_obst); i++)
	//									{
	//										sx=rand() % 2 +1;
	//										sy=rand() % 2 +1;
	//									xx=rand() % (int)envMap_.max_for_obstacles;
	//									yy=rand() % (int)envMap_.max_for_obstacles;
	//									if(sx-1==1)xx*=-1;
	//									if(sy-1==1)yy*=-1;
	//									goal =PFP::VEC3(-xx,yy,0);
	//
	//												posInit=PFP::VEC3(0,0,0);
	//												//posInit[0] -= 200.0f;
	//												posInit[1] += yy;
	//												posInit[0] += xx;
	//												possiblepos=posInit;
	//												Dart d1 =envMap_.getBelongingCell(possiblepos);
	//												if (envMap_.buildingMark.isMarked(d1))
	//												{
	//												if (((possiblepos[0]+envMap_.sideSize) <envMap_.max_for_obstacles) && ((possiblepos[1]+envMap_.sideSize) <envMap_.max_for_obstacles)) possiblepos += VEC3(envMap_.sideSize,envMap_.sideSize,0);
	//												else if (((possiblepos[0]-envMap_.sideSize) <envMap_.max_for_obstacles) && ((possiblepos[1]-envMap_.sideSize) <envMap_.max_for_obstacles)) possiblepos += VEC3(-envMap_.sideSize,-envMap_.sideSize,0);
	//												else if (((possiblepos[0]-envMap_.sideSize) <envMap_.max_for_obstacles) && ((possiblepos[1]+envMap_.sideSize) <envMap_.max_for_obstacles)) possiblepos += VEC3(-envMap_.sideSize,envMap_.sideSize,0);
	//												else possiblepos += VEC3(envMap_.sideSize,-envMap_.sideSize,0);
	//												}
	//												posInit=possiblepos;
	//
	//												vPos.clear();
	//												vPos.push_back(posInit);
	//												posInit[0] -= xSize/2;
	//											//	vPos.push_back(posInit);
	//												posInit[1] -= ySize/2;
	//												vPos.push_back(posInit);
	//												posInit[0] += xSize;
	//											//	posInit[1] += ySize;
	//												vPos.push_back(posInit);
	//											//	posInit[0] -= xSize;
	//											//	posInit[1] += ySize;
	//											//		vPos.push_back(posInit);
	//
	//
	//											//	CGoGNout << "ligne "<< i << vPos[i] << CGoGNendl;
	//
	//												 mo4= new MovingObstacle(i,&envMap_, vPos,goal,0);
	//												movingObstacles_.push_back(mo4);
	//									}


	//				xx=-45;
	//				yy=245;
	//				goal =PFP::VEC3(xx,-yy,0);
	//
	//							posInit=PFP::VEC3(0,0,0);
	//							//posInit[0] -= 200.0f;
	//							posInit[1] += yy;
	//							posInit[0] += xx;
	//
	//
	//							vPos.clear();
	//							vPos.push_back(posInit);
	//							posInit[0] -= xSize/2;
	//						//	vPos.push_back(posInit);
	//							posInit[1] -= ySize/2;
	//							vPos.push_back(posInit);
	//							posInit[0] += xSize;
	//						//	posInit[1] += ySize;
	//							vPos.push_back(posInit);
	//						//	posInit[0] -= xSize;
	//						//	posInit[1] += ySize;
	//						//		vPos.push_back(posInit);
	//
	//
	//						//	CGoGNout << "ligne "<< i << vPos[i] << CGoGNendl;
	//
	//							 mo4= new MovingObstacle(0,&envMap_, vPos,goal,0);
	//							movingObstacles_.push_back(mo4);

	//							xx=-150;
	//							yy=50;
	//							goal =PFP::VEC3(xx,-yy,0);
	//
	//										posInit=PFP::VEC3(0,0,0);
	//										//posInit[0] -= 200.0f;
	//										posInit[1] += yy;
	//										posInit[0] += xx;
	//
	//
	//										vPos.clear();
	//										vPos.push_back(posInit);
	//										posInit[0] -= xSize;
	//									//	vPos.push_back(posInit);
	//										posInit[1] -= ySize;
	//										vPos.push_back(posInit);
	//										posInit[0] += 2*xSize;
	//									//	posInit[1] += ySize;
	//										vPos.push_back(posInit);
	//									//	posInit[0] -= xSize;
	//									//	posInit[1] += ySize;
	//									//		vPos.push_back(posInit);
	//
	//
	//									//	CGoGNout << "ligne "<< i << vPos[i] << CGoGNendl;
	//
	//										 mo4= new MovingObstacle(1,&envMap_, vPos,goal,0);
	//										movingObstacles_.push_back(mo4);


	//
	//					xx=-245;
	//					yy=245;
	//					goal =PFP::VEC3(xx,-yy,0);
	//
	//								posInit=PFP::VEC3(0,0,0);
	//								//posInit[0] -= 200.0f;
	//								posInit[1] += yy;
	//								posInit[0] += xx;
	//
	//
	//								vPos.clear();
	//								vPos.push_back(posInit);
	//								posInit[0] -= xSize/2;
	//							//	vPos.push_back(posInit);
	//								posInit[1] -= ySize/2;
	//								vPos.push_back(posInit);
	//								posInit[0] += xSize;
	//							//	posInit[1] += ySize;
	//								vPos.push_back(posInit);
	//							//	posInit[0] -= xSize;
	//							//	posInit[1] += ySize;
	//							//		vPos.push_back(posInit);
	//
	//
	//							//	CGoGNout << "ligne "<< i << vPos[i] << CGoGNendl;
	//
	//								 mo4= new MovingObstacle(2,&envMap_, vPos,goal,0);
	//								movingObstacles_.push_back(mo4);
	//
	//
	//					xx=145;
	//					yy=245;
	//					goal =PFP::VEC3(xx,-yy,0);
	//								posInit=PFP::VEC3(0,0,0);
	//								//posInit[0] -= 200.0f;
	//								posInit[1] += yy;
	//								posInit[0] += xx;
	//
	//
	//								vPos.clear();
	//								vPos.push_back(posInit);
	//								posInit[0] -= xSize/2;
	//								vPos.push_back(posInit);
	//								posInit[0] -= xSize/2;
	//								posInit[1] -= ySize/2;
	//								vPos.push_back(posInit);
	//								posInit[0] += xSize/2;
	//								posInit[1] -= ySize/2;
	//								vPos.push_back(posInit);
	//								posInit[0] += xSize;
	//							//	posInit[1] += ySize;
	//								vPos.push_back(posInit);
	//								posInit[0] += xSize/2;
	//								posInit[1] += ySize/2;
	//								vPos.push_back(posInit);
	//								posInit[0] -= xSize/2;
	//								posInit[1] += ySize/2;
	//								vPos.push_back(posInit);
	//
	//
	//							//	CGoGNout << "ligne "<< i << vPos[i] << CGoGNendl;
	//
	//								 mo4= new MovingObstacle(3,&envMap_, vPos,goal,0);
	//								movingObstacles_.push_back(mo4);
	//							xx=45;
	//							yy=145;
	//							goal =PFP::VEC3(xx,-yy,0);
	//							posInit=PFP::VEC3(0,0,0);
	//							//posInit[0] -= 200.0f;
	//							posInit[1] += yy;
	//							posInit[0] += xx;
	//
	//
	//							vPos.clear();
	//							vPos.push_back(posInit);
	//							posInit[0] -= xSize/2;
	//							vPos.push_back(posInit);
	//							posInit[0] -= xSize/2;
	//							posInit[1] -= ySize/2;
	//							vPos.push_back(posInit);
	//							posInit[0] += xSize/2;
	//							posInit[1] -= ySize/2;
	//							vPos.push_back(posInit);
	//							posInit[0] += xSize;
	//						//	posInit[1] += ySize;
	//							vPos.push_back(posInit);
	//							posInit[0] += xSize/2;
	//							posInit[1] += ySize/2;
	//							vPos.push_back(posInit);
	//							posInit[0] -= xSize/2;
	//							posInit[1] += ySize/2;
	//							vPos.push_back(posInit);
	//
	//
	//						//	CGoGNout << "ligne "<< i << vPos[i] << CGoGNendl;
	//
	//							 mo4= new MovingObstacle(4,&envMap_, vPos,goal,0);
	//							movingObstacles_.push_back(mo4);


	//
pitiot's avatar
pitiot committed
1031
1032

}