simulator.cpp 9.64 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() : timeStep_(0.2f), globalTime_(0.0f), nbSteps_(0)
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
	nbStepsPerUnit_ = 1 / timeStep_;

Pierre Kraemer's avatar
Pierre Kraemer committed
8
	envMap_.init();
Pierre Kraemer's avatar
Pierre Kraemer committed
9

Pierre Kraemer's avatar
Pierre Kraemer committed
10
	std::cout << "setup scenario" << std::endl;
Thomas's avatar
Thomas committed
11
//	importAgents("myAgents.pos");
Thomas's avatar
Thomas committed
12
//	setupScenario(700);
Pierre Kraemer's avatar
Pierre Kraemer committed
13
//	setupCircleScenario(500);
Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
14
	setupCityScenario(
Pierre Kraemer's avatar
Pierre Kraemer committed
15
16
17
18
		-1.0f * (12 * (70.0f / 2.0f) - 10),
		-1.0f * (12 * (70.0f / 2.0f) - 10),
		20,
		20
Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
19
	);
20
//	addPathsToAgents();
21
22
23
24

	unsigned nbAgents = agents_.size();

	for(unsigned int i = 0; i < nbAgents; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
25
26
27
28
	{
		agents_[i]->updateObstacleNeighbors();
		agents_[i]->updateAgentNeighbors();
	}
29

Pierre Kraemer's avatar
Pierre Kraemer committed
30
31
//	tc1 = new ThreadUpdateInfo(agents_, 0, nbAgents / 4);
//	tc2 = new ThreadUpdateInfo(agents_, nbAgents / 4, nbAgents / 2);
32
33
//	tc3 = new ThreadUpdateInfo(agents_, nbAgents / 2, nbAgents * 3 / 4);
//	tc4 = new ThreadUpdateInfo(agents_, nbAgents * 3 / 4, nbAgents);
Thomas's avatar
Thomas committed
34

Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
35
	envMap_.subdivideToProperLevel();
Pierre Kraemer's avatar
Pierre Kraemer committed
36
37
38
39
40
41
42
43
44
45
46
47
}

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
48
	envMap_.map.setCurrentLevel(0);
49

Thomas's avatar
Thomas committed
50
51
52
	for (unsigned int i = 0; i < agents_.size(); ++i)
	{
		agents_[i]->updateObstacleNeighbors();
Thomas's avatar
Thomas committed
53
54

		envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel());
Thomas's avatar
Thomas committed
55
		agents_[i]->updateAgentNeighbors();
Thomas's avatar
Thomas committed
56
57
		envMap_.map.setCurrentLevel(0);

Thomas's avatar
Thomas committed
58
59
60
		agents_[i]->computePrefVelocity();
		agents_[i]->computeNewVelocity();
	}
61

Thomas's avatar
Thomas committed
62
63
//	boost::thread thread1(&ThreadUpdateInfo::run, tc1);
//	boost::thread thread2(&ThreadUpdateInfo::run, tc2);
Pierre Kraemer's avatar
Pierre Kraemer committed
64
65
66
//	boost::thread thread3(&ThreadUpdateInfo::run, tc3);
//	boost::thread thread4(&ThreadUpdateInfo::run, tc4);
//
Thomas's avatar
Thomas committed
67
68
//	thread1.join();
//	thread2.join();
Pierre Kraemer's avatar
Pierre Kraemer committed
69
70
//	thread3.join();
//	thread4.join();
71

Pierre Kraemer's avatar
Pierre Kraemer committed
72
	envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel());
Pierre Kraemer's avatar
Pierre Kraemer committed
73
74
75
76

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

Pierre Kraemer's avatar
Pierre Kraemer committed
78
		agents_[i]->update();
Pierre Kraemer's avatar
Pierre Kraemer committed
79

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

Pierre Kraemer's avatar
Pierre Kraemer committed
92
93
	}

Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
94
	envMap_.updateMap();
Pierre Kraemer's avatar
Pierre Kraemer committed
95
96

	globalTime_ += timeStep_;
Pierre Kraemer's avatar
Pierre Kraemer committed
97
	++nbSteps_;
Pierre Kraemer's avatar
Pierre Kraemer committed
98

Pierre Kraemer's avatar
Pierre Kraemer committed
99
//	if(nbSteps % 2000 == 0)
Thomas's avatar
Thomas committed
100
101
102
103
//	{
//		std::cout << "swap goals (globalTime -> " << globalTime_ << ")" << std::endl ;
//		swapAgentsGoals();
//	}
Thomas's avatar
Thomas committed
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
}

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;

Pierre Kraemer's avatar
Pierre Kraemer committed
120
	float r = 400.0f;
Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
121
122
	for (unsigned int i = 0; i < nbMaxAgent; ++i)
	{
Thomas's avatar
Thomas committed
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
		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)
{
Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
138
139
	float xCornerDown = startX;
	float yCornerDown = startY;
Thomas's avatar
Thomas committed
140

Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
141
	for (int i = 0; i < nbLines; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
142
	{
Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
143
144
145
146
147
148
149
150
		for (int j = 0; j < nbRank; ++j)
		{
			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(-1.0f * posagent);
			agents_.back()->curGoal_ = 0;
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
151
	}
Thomas's avatar
Thomas committed
152
153
154
155
156

	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
157
158
}

159
void Simulator::setupScenario(unsigned int nbMaxAgent)
Pierre Kraemer's avatar
Pierre Kraemer committed
160
161
162
163
164
165
{
	/*
	* 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
166
	CellMarker filled(envMap_.map,FACE);
Pierre Kraemer's avatar
Pierre Kraemer committed
167

Pierre Kraemer's avatar
Pierre Kraemer committed
168
169
	unsigned int nbx = 10;
	unsigned int nby = 10;
170

Pierre Kraemer's avatar
Pierre Kraemer committed
171
	unsigned int bMax = nbMaxAgent / (nbx*nby);
172
173

	for (unsigned int i = 0; i < bMax && d != envMap_.map.end(); ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
174
175
176
177
178
179
180
181
	{
		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
182
				filled.mark(d);
Pierre Kraemer's avatar
Pierre Kraemer committed
183
				pos = Algo::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position);
184
				pos[2] = 0;
Pierre Kraemer's avatar
Pierre Kraemer committed
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
				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
202
//					agents_.back()->goals_.push_back(VEC3(0,0,0));
Pierre Kraemer's avatar
Pierre Kraemer committed
203
					agents_.back()->goals_.push_back(-1.0f * posagent);
Pierre Kraemer's avatar
Pierre Kraemer committed
204
					agents_.back()->curGoal_ = 1;
Pierre Kraemer's avatar
Pierre Kraemer committed
205
206
207
208
209
210
211
212
213
				}
			}
		}
	}
	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
214
		envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d);
Pierre Kraemer's avatar
Pierre Kraemer committed
215
}
216

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

Pierre Kraemer's avatar
Pierre Kraemer committed
225
226
		Dart dStart = agents_[i]->part_.d;
		Dart dStop = dStart;
227
		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
228
		{
Thomas's avatar
Thomas committed
229
			envMap_.map.next(dStop);
Pierre Kraemer's avatar
Pierre Kraemer committed
230
231
232
			if(dStop == envMap_.map.end())
				dStop = envMap_.map.begin();
		}
233
234
235

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

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

Pierre Kraemer's avatar
Pierre Kraemer committed
245
		Dart dStop2 = dStop;
246
		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
247
		{
Thomas's avatar
Thomas committed
248
			envMap_.map.next(dStop2);
Pierre Kraemer's avatar
Pierre Kraemer committed
249
250
251
			if(dStop2 == envMap_.map.end())
				dStop2 = envMap_.map.begin();
		}
252
253
254

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

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

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

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

Pierre Kraemer's avatar
Pierre Kraemer committed
277
278
279
280
281
282
283
284
285
286
287
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
288
289
	while ( myfile.good() )
	{
Pierre Kraemer's avatar
Pierre Kraemer committed
290
291
		getline(myfile,line);

Pierre Kraemer's avatar
Pierre Kraemer committed
292
293
		if(line.size() > 1)
		{
Pierre Kraemer's avatar
Pierre Kraemer committed
294
295
296
			std::istringstream iss(line);
			float x,y,z;

Pierre Kraemer's avatar
Pierre Kraemer committed
297
298
299
300
301
			iss >> x;
			iss >> y;
			iss >> z;

			VEC3 pos(x, y, z);
Pierre Kraemer's avatar
Pierre Kraemer committed
302
303
304
305
306
			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
307
			agents_.back()->curGoal_ = 1;
Pierre Kraemer's avatar
Pierre Kraemer committed
308
309
310
		}
	}

Pierre Kraemer's avatar
Pierre Kraemer committed
311
312
	swapAgentsGoals();

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

	myfile.close();
	return true;
}

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

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

	out.close();
	return true;
}

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

Thomas's avatar
Thomas committed
353
354
355
356
357
358
359
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
360
	}
Thomas's avatar
Thomas committed
361
362

	return bb;
Pierre Kraemer's avatar
Pierre Kraemer committed
363
}
364

Thomas's avatar
Thomas committed
365