simulator.cpp 10.1 KB
Newer Older
Pierre Kraemer's avatar
Pierre Kraemer committed
1
2
#include "simulator.h"

Pierre Kraemer's avatar
Pierre Kraemer committed
3
Simulator::Simulator() : globalTime_(0.0f), timeStep_(0.2f)
Pierre Kraemer's avatar
Pierre Kraemer committed
4
{
Pierre Kraemer's avatar
Pierre Kraemer committed
5
	srand(10);
Pierre Kraemer's avatar
Pierre Kraemer committed
6
7
	envMap_.init();
	std::cout << "setup scenario" << std::endl;
Thomas's avatar
Thomas committed
8
//	importAgents("myAgents.pos");
Thomas's avatar
Thomas committed
9
10
//	setupScenario(700);
	setupCircleScenario(1000);
Thomas's avatar
Thomas committed
11
//	setupCityScenario(-1.0f*(24*(70/2.0f)-10),-1.0f*(24*(70.0f/2.0f)-10),167,5);
12
//	addPathsToAgents();
13
14
15
16

	unsigned nbAgents = agents_.size();

	for(unsigned int i = 0; i < nbAgents; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
17
18
19
20
	{
		agents_[i]->updateObstacleNeighbors();
		agents_[i]->updateAgentNeighbors();
	}
21

Thomas's avatar
Thomas committed
22
23
//	tc1 = new ThreadUpdateInfo(agents_, 0, nbAgents / 2);
//	tc2 = new ThreadUpdateInfo(agents_, nbAgents / 2, nbAgents );
24
25
//	tc3 = new ThreadUpdateInfo(agents_, nbAgents / 2, nbAgents * 3 / 4);
//	tc4 = new ThreadUpdateInfo(agents_, nbAgents * 3 / 4, nbAgents);
Thomas's avatar
Thomas committed
26

Thomas's avatar
Thomas committed
27
//	envMap_.subdivideToProperLevel();
Pierre Kraemer's avatar
Pierre Kraemer committed
28
29
30
31
32
33
34
35
36
37
38
39
}

Simulator::~Simulator()
{
	for (unsigned int i = 0; i < agents_.size(); ++i)
		delete agents_[i];
}

void Simulator::doStep()
{
	envMap_.clearUpdateCandidates();

Pierre Kraemer's avatar
Pierre Kraemer committed
40
	envMap_.map.setCurrentLevel(0);
41

Thomas's avatar
Thomas committed
42
43
44
	for (unsigned int i = 0; i < agents_.size(); ++i)
	{
		agents_[i]->updateObstacleNeighbors();
Thomas's avatar
Thomas committed
45
46

		envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel());
Thomas's avatar
Thomas committed
47
		agents_[i]->updateAgentNeighbors();
Thomas's avatar
Thomas committed
48
49
		envMap_.map.setCurrentLevel(0);

Thomas's avatar
Thomas committed
50
51
52
		agents_[i]->computePrefVelocity();
		agents_[i]->computeNewVelocity();
	}
53

Thomas's avatar
Thomas committed
54
55
//	boost::thread thread1(&ThreadUpdateInfo::run, tc1);
//	boost::thread thread2(&ThreadUpdateInfo::run, tc2);
56
57
58
59
60
61
62
63

//	std::vector<Dart> oldDarts;
//	oldDarts.reserve(agents_.size());
//	for (unsigned int i = 0; i < agents_.size(); ++i)
//	{
//		oldDarts.push_back(agents_[i]->part_.d);
//	}

Thomas's avatar
Thomas committed
64
65
//	thread1.join();
//	thread2.join();
66

Pierre Kraemer's avatar
Pierre Kraemer committed
67
	envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel());
Pierre Kraemer's avatar
Pierre Kraemer committed
68
69
70
71

	for (unsigned int i = 0; i < agents_.size(); ++i)
	{
		Dart oldFace = agents_[i]->part_.d;
Pierre Kraemer's avatar
Pierre Kraemer committed
72

Thomas's avatar
Thomas committed
73
74
//		if(!envMap_.map.isDartValid(agents_[i]->part_.d))
//			std::cout << "  AGENT PART INVALID DART before update" << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
75
		agents_[i]->update();
Thomas's avatar
Thomas committed
76
77
//		if(!envMap_.map.isDartValid(agents_[i]->part_.d))
//			std::cout << "  AGENT PART INVALID DART after update" << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
78

Pierre Kraemer's avatar
Pierre Kraemer committed
79
//		if(envMap_.map.getEmbedding(oldFace, FACE) != envMap_.map.getEmbedding(agents_[i]->part_.d, FACE))
Pierre Kraemer's avatar
Pierre Kraemer committed
80
81
82
		if(agents_[i]->part_.crossCell != CGoGN::Algo::MovingObjects::NO_CROSS)
//		switch(agents_[i]->part_.crossCell)
		{
Pierre Kraemer's avatar
Pierre Kraemer committed
83
//			case CGoGN::Algo::MovingObjects::CROSS_EDGE :
Pierre Kraemer's avatar
Pierre Kraemer committed
84
85
86
//				envMap_.agentChangeFaceThroughEdge(agents_[i]);
//				envMap_.addCoarsenCandidate(oldFace);
//				envMap_.addRefineCandidate(agents_[i]->part_.d);
Pierre Kraemer's avatar
Pierre Kraemer committed
87
88
89
//				break;
//			case CGoGN::Algo::MovingObjects::CROSS_OTHER :
				envMap_.agentChangeFace(agents_[i], oldFace);
90
//				envMap_.agentChangeFace(agents_[i], oldDarts[i]);
Pierre Kraemer's avatar
Pierre Kraemer committed
91
//				break;
Pierre Kraemer's avatar
Pierre Kraemer committed
92
93
		}

Pierre Kraemer's avatar
Pierre Kraemer committed
94
95
	}

Thomas's avatar
Thomas committed
96
//	envMap_.updateMap();
Pierre Kraemer's avatar
Pierre Kraemer committed
97
98

	globalTime_ += timeStep_;
Pierre Kraemer's avatar
Pierre Kraemer committed
99

Thomas's avatar
Thomas committed
100
101
102
103
104
//	if(int(globalTime_ / timeStep_) % 2000 == 0)
//	{
//		std::cout << "swap goals (globalTime -> " << globalTime_ << ")" << std::endl ;
//		swapAgentsGoals();
//	}
Thomas's avatar
Thomas committed
105
106
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
133
134
135
136
137
138
139
140
141
142
143
144
145
146
}

bool Simulator::reachedGoal()
{
  /* Check if all agents have reached their goals. */
	for (unsigned int i = 0; i < agents_.size(); ++i)
		if ((agents_[i]->getPosition() - agents_[i]->goals_[agents_[i]->curGoal_]).norm2() > agents_[i]->radius_ * agents_[i]->radius_)
			return false;

  return true;
}

void Simulator::setupCircleScenario(unsigned int nbMaxAgent)
{
	float pi = 3.14159265358979323846f;

	float r = 800.0f;
	for (int i = 0; i < nbMaxAgent; ++i) {
		VEC3 posagent(std::cos(i * 2.0f * pi / float(nbMaxAgent))*r,std::sin(i * 2.0f * pi / float(nbMaxAgent))*r,0);
		Dart dCell = envMap_.getBelongingCell(posagent);
		agents_.push_back(new Agent(this, posagent, dCell));
		agents_.back()->goals_.push_back(-1.0f * posagent);
		agents_.back()->curGoal_ = 0;
	}

	std::cout << "nb agents : " << agents_.size() << std::endl;

	for(unsigned int i = 0; i < agents_.size(); ++i)
		envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d);
}

void Simulator::setupCityScenario(float startX, float startY, int nbLines , int nbRank)
{
  float xCornerDown = startX;
  float yCornerDown = startY;

  float goalX = 75.0f;
  float goalY = 75.0f;

  for (int i = 0; i < nbLines; ++i)
  {
	for (int j = 0; j < nbRank; ++j)
Pierre Kraemer's avatar
Pierre Kraemer committed
147
	{
Thomas's avatar
Thomas committed
148
149
150
151
152
153
		VEC3 posagent(xCornerDown + i * 10.0f,  yCornerDown + j * 10.0f,0.0f);
		Dart dCell = envMap_.getBelongingCell(posagent);
		agents_.push_back(new Agent(this, posagent, dCell));
//		agents_.back()->goals_.push_back(VEC3(xCornerDown + (i+1) * 10.0f,  -yCornerDown - j * 10.0f,0.0f));
		agents_.back()->goals_.push_back(-1.0f * posagent);
		agents_.back()->curGoal_ = 0;
Pierre Kraemer's avatar
Pierre Kraemer committed
154
	}
Thomas's avatar
Thomas committed
155
156
157
158
159
160
  }

	std::cout << "nb agents : " << agents_.size() << std::endl;

	for(unsigned int i = 0; i < agents_.size(); ++i)
		envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d);
Pierre Kraemer's avatar
Pierre Kraemer committed
161
162
}

163
void Simulator::setupScenario(unsigned int nbMaxAgent)
Pierre Kraemer's avatar
Pierre Kraemer committed
164
165
166
167
168
169
{
	/*
	* Add agents, specifying their start position, and store their goals on the
	* opposite side of the environment.
	*/
	Dart d = envMap_.map.begin();
Pierre Kraemer's avatar
Pierre Kraemer committed
170
	CellMarker filled(envMap_.map,FACE);
Pierre Kraemer's avatar
Pierre Kraemer committed
171

Pierre Kraemer's avatar
Pierre Kraemer committed
172
173
	unsigned int nbx = 10;
	unsigned int nby = 10;
174

Pierre Kraemer's avatar
Pierre Kraemer committed
175
	unsigned int bMax = nbMaxAgent / (nbx*nby);
176
177

	for (unsigned int i = 0; i < bMax && d != envMap_.map.end(); ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
178
179
180
181
182
183
184
185
	{
		bool found = false;
		VEC3 pos;
		Dart dCell;
		while(!found && d != envMap_.map.end())
		{
			if(!filled.isMarked(d) && !envMap_.buildingMark.isMarked(d))
			{
Thomas's avatar
Thomas committed
186
				filled.mark(d);
Pierre Kraemer's avatar
Pierre Kraemer committed
187
				pos = Algo::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position);
188
				pos[2] = 0;
Pierre Kraemer's avatar
Pierre Kraemer committed
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
				dCell = d;
				found = true;
			}
			envMap_.map.next(d);
		}

		if(found)
		{
			float ecart = 3.0f;
			VEC3 posinit = VEC3(pos[0] - (float(nbx)/2.0f * ecart), 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)
				{
					VEC3 posagent = posinit + VEC3(ecart * curx, ecart * cury, 0.0f);
					agents_.push_back(new Agent(this, posagent, dCell));
					agents_.back()->goals_.push_back(posagent);
Thomas's avatar
Thomas committed
206
//					agents_.back()->goals_.push_back(VEC3(0,0,0));
Pierre Kraemer's avatar
Pierre Kraemer committed
207
					agents_.back()->goals_.push_back(-1.0f * posagent);
Pierre Kraemer's avatar
Pierre Kraemer committed
208
					agents_.back()->curGoal_ = 1;
Pierre Kraemer's avatar
Pierre Kraemer committed
209
210
211
212
213
214
215
216
217
				}
			}
		}
	}
	std::cout << "nb agents : " << agents_.size() << std::endl;

	swapAgentsGoals();

	for(unsigned int i = 0; i < agents_.size(); ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
218
		envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d);
Pierre Kraemer's avatar
Pierre Kraemer committed
219
}
220

Pierre Kraemer's avatar
Pierre Kraemer committed
221
222
void Simulator::addPathsToAgents()
{
Thomas's avatar
Thomas committed
223
	//city
Pierre Kraemer's avatar
Pierre Kraemer committed
224
	unsigned int dartDistForPath = 50;
Pierre Kraemer's avatar
Pierre Kraemer committed
225
226
227
	for(unsigned int i = 0; i < agents_.size(); ++i)
	{
		agents_[i]->goals_.clear();
228

Pierre Kraemer's avatar
Pierre Kraemer committed
229
230
		Dart dStart = agents_[i]->part_.d;
		Dart dStop = dStart;
231
		for(unsigned int j = 0; envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath+rand()*20 || envMap_.map.sameFace(dStop,dStart); ++j)
Pierre Kraemer's avatar
Pierre Kraemer committed
232
		{
Thomas's avatar
Thomas committed
233
			envMap_.map.next(dStop);
Pierre Kraemer's avatar
Pierre Kraemer committed
234
235
236
			if(dStop == envMap_.map.end())
				dStop = envMap_.map.begin();
		}
237
238
239

		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map, envMap_.position, dStart, dStop, envMap_.buildingMark);

Pierre Kraemer's avatar
Pierre Kraemer committed
240
		for(std::vector<Dart>::iterator it = path.begin(); it != path.end(); ++it)
241
242
		{
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
Thomas's avatar
Thomas committed
243
244
//			VEC3 dest = envMap_.position[envMap_.map.phi_1(*it)] + envMap_.position[envMap_.map.phi_1(envMap_.map.phi_1(*it))];
//			dest /= 2.0f;
245
246
247
			dest[2]=0;
			agents_[i]->goals_.push_back(dest);
		}
248

Pierre Kraemer's avatar
Pierre Kraemer committed
249
		Dart dStop2 = dStop;
250
		for(unsigned int j = 0; envMap_.buildingMark.isMarked(dStop2) || j < dartDistForPath+rand()*20 || envMap_.map.sameFace(dStop,dStop2) || envMap_.map.sameFace(dStop2,dStart); ++j)
Pierre Kraemer's avatar
Pierre Kraemer committed
251
		{
Thomas's avatar
Thomas committed
252
			envMap_.map.next(dStop2);
Pierre Kraemer's avatar
Pierre Kraemer committed
253
254
255
			if(dStop2 == envMap_.map.end())
				dStop2 = envMap_.map.begin();
		}
256
257
258

		path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map, envMap_.position, dStop, dStop2, envMap_.buildingMark);

Pierre Kraemer's avatar
Pierre Kraemer committed
259
		for(std::vector<Dart>::iterator it = path.begin(); it!=path.end();++it)
260
261
		{
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
Thomas's avatar
Thomas committed
262
263
//			VEC3 dest = envMap_.position[envMap_.map.phi_1(*it)] + envMap_.position[envMap_.map.phi_1(envMap_.map.phi_1(*it))];
//			dest /= 2.0f;
264
265
266
			dest[2]=0;
			agents_[i]->goals_.push_back(dest);
		}
267
268
269

		path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map, envMap_.position, dStop2, dStart, envMap_.buildingMark);

Pierre Kraemer's avatar
Pierre Kraemer committed
270
		for(std::vector<Dart>::iterator it = path.begin(); it!=path.end();++it)
271
272
		{
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
Thomas's avatar
Thomas committed
273
274
//			VEC3 dest = envMap_.position[envMap_.map.phi_1(*it)] + envMap_.position[envMap_.map.phi_1(envMap_.map.phi_1(*it))];
//			dest /= 2.0f;
275
276
277
			dest[2]=0;
			agents_[i]->goals_.push_back(dest);
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
278
279
	}
}
280

Pierre Kraemer's avatar
Pierre Kraemer committed
281
282
283
284
285
286
287
288
289
290
291
bool Simulator::importAgents(std::string filename)
{
	std::ifstream myfile(filename.c_str(), std::ios::in);

	if (!myfile.good())
	{
		std::cerr << "(export) Unable to open file " << filename << std::endl;
		return false;
	}

	std::string line,token;
Pierre Kraemer's avatar
Pierre Kraemer committed
292
293
	while ( myfile.good() )
	{
Pierre Kraemer's avatar
Pierre Kraemer committed
294
295
		getline(myfile,line);

Pierre Kraemer's avatar
Pierre Kraemer committed
296
297
		if(line.size() > 1)
		{
Pierre Kraemer's avatar
Pierre Kraemer committed
298
299
300
			std::istringstream iss(line);
			float x,y,z;

Pierre Kraemer's avatar
Pierre Kraemer committed
301
302
303
304
305
			iss >> x;
			iss >> y;
			iss >> z;

			VEC3 pos(x, y, z);
Pierre Kraemer's avatar
Pierre Kraemer committed
306
307
308
309
310
			Dart dCell = envMap_.getBelongingCell(pos);

			agents_.push_back(new Agent(this, pos, dCell));
			agents_.back()->goals_.push_back(pos);
			agents_.back()->goals_.push_back(-1.0f * pos);
Pierre Kraemer's avatar
Pierre Kraemer committed
311
			agents_.back()->curGoal_ = 1;
Pierre Kraemer's avatar
Pierre Kraemer committed
312
313
314
		}
	}

Pierre Kraemer's avatar
Pierre Kraemer committed
315
316
	swapAgentsGoals();

Pierre Kraemer's avatar
Pierre Kraemer committed
317
	for (unsigned int i = 0; i < agents_.size(); ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
318
		envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d);
Pierre Kraemer's avatar
Pierre Kraemer committed
319
320
321
322
323
324
325
326

	myfile.close();
	return true;
}

bool Simulator::exportAgents(std::string filename)
{
	std::ofstream out(filename.c_str(), std::ios::out);
327
328
	if (!out.good())
	{
Pierre Kraemer's avatar
Pierre Kraemer committed
329
330
331
332
		std::cerr << "(export) Unable to open file " << filename << std::endl;
		return false;
	}

333
	for (unsigned int i = 0; i < agents_.size(); ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
334
335
336
337
338
339
340
341
		out << agents_[i]->part_.m_position << std::endl;

	out.close();
	return true;
}

void Simulator::swapAgentsGoals()
{
Pierre Kraemer's avatar
Pierre Kraemer committed
342
343
344
345
	unsigned int nbAgents = agents_.size();
	for(unsigned int i = 0; i < nbAgents; ++i)
	{
		unsigned int r = rand() % nbAgents;
Thomas's avatar
Thomas committed
346
347
348
349
350
351
352
353
354
355
//		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;
//		}

		std::swap(agents_[i]->goals_,agents_[r]->goals_);
	}
}
Pierre Kraemer's avatar
Pierre Kraemer committed
356

Thomas's avatar
Thomas committed
357
358
359
360
361
362
363
Geom::BoundingBox<VEC3> Simulator::getAgentsBB()
{
	Geom::BoundingBox<VEC3> bb(agents_[0]->getPosition());
	unsigned int nbAgents = agents_.size();
	for(unsigned int i = 1; i < nbAgents; ++i)
	{
		bb.addPoint(agents_[i]->getPosition());
Pierre Kraemer's avatar
Pierre Kraemer committed
364
	}
Thomas's avatar
Thomas committed
365
366

	return bb;
Pierre Kraemer's avatar
Pierre Kraemer committed
367
}
368

Thomas's avatar
Thomas committed
369