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

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
9
//	importAgents("myAgents.pos");
	setupScenario(15000);
10
//	addPathsToAgents();
Pierre Kraemer's avatar
Pierre Kraemer committed
11
12
13
14
15
16
17
18
19
20
21
22
23
	for(unsigned int i = 0; i < agents_.size(); ++i)
	{
		agents_[i]->updateObstacleNeighbors();
		agents_[i]->updateAgentNeighbors();
	}
}

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

24
25
class ThreadUpdateInfo
{
26
27
28
29
30
31
public :
	std::vector<Agent*> ag_;
	unsigned int bMin_;
	unsigned int bMax_;

	// Constructor
32
33
34
35
36
	ThreadUpdateInfo(std::vector<Agent*> agents, unsigned int bMin, unsigned int bMax) :
		bMin_(bMin),
		bMax_(bMax)
	{
		ag_.insert(ag_.end(), agents.begin() + bMin, agents.begin() + bMax);
37
38
39
40
41
	}

	// Destructor
	~ThreadUpdateInfo() { }

42
43
	void run()
	{
44
45
46
47
48
49
50
51
52
53
54
	    // Thread execution stuff goes here
		for (unsigned int i = 0; i < ag_.size(); ++i)
		{
			ag_[i]->updateObstacleNeighbors();
			ag_[i]->updateAgentNeighbors();
			ag_[i]->computePrefVelocity();
			ag_[i]->computeNewVelocity();
		}
	}
};

55
56
class ThreadUpdatePos
{
57
58
59
60
61
62
public :
	std::vector<Agent*> ag_;
	unsigned int bMin_;
	unsigned int bMax_;

	// Constructor
63
64
65
66
67
	ThreadUpdatePos(std::vector<Agent*> agents, unsigned int bMin, unsigned int bMax) :
		bMin_(bMin),
		bMax_(bMax)
	{
		ag_.insert(ag_.end(), agents.begin() + bMin, agents.begin() + bMax);
68
69
70
71
72
	}

	// Destructor
	~ThreadUpdatePos() { }

73
74
	void run()
	{
75
76
77
78
79
80
81
82
	    // Thread execution stuff goes here
		for (unsigned int i = 0; i < ag_.size(); ++i)
		{
			ag_[i]->update();
		}
	}
};

Pierre Kraemer's avatar
Pierre Kraemer committed
83
84
85
86
void Simulator::doStep()
{
	envMap_.clearUpdateCandidates();

Pierre Kraemer's avatar
Pierre Kraemer committed
87
	envMap_.map.setCurrentLevel(0);
88

89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
	for (unsigned int i = 0; i < agents_.size(); ++i)
	{
		agents_[i]->updateObstacleNeighbors();
		agents_[i]->updateAgentNeighbors();
		agents_[i]->computePrefVelocity();
		agents_[i]->computeNewVelocity();
	}

//	unsigned nbAgents = agents_.size();
//	ThreadUpdateInfo tc1(agents_, 0, nbAgents / 4);
//	ThreadUpdateInfo tc2(agents_, nbAgents / 4, nbAgents / 2);
//	ThreadUpdateInfo tc3(agents_, nbAgents / 2, nbAgents * 3 / 4);
//	ThreadUpdateInfo tc4(agents_, nbAgents * 3 / 4, nbAgents);
//	boost::thread thread1(&ThreadUpdateInfo::run, &tc1);
//	boost::thread thread2(&ThreadUpdateInfo::run, &tc2);
//	boost::thread thread3(&ThreadUpdateInfo::run, &tc3);
//	boost::thread thread4(&ThreadUpdateInfo::run, &tc4);
106
107
108
109
110
111
112
113

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

114
115
116
117
//	thread1.join();
//	thread2.join();
//	thread3.join();
//	thread4.join();
118

Pierre Kraemer's avatar
Pierre Kraemer committed
119
	envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel());
Pierre Kraemer's avatar
Pierre Kraemer committed
120

121
122
//	ThreadUpdatePos tc3(agents_,0,agents_.size()/2);
//	ThreadUpdatePos tc4(agents_,agents_.size()/2,agents_.size());
123
124
125
126
//	boost::thread thread3(&ThreadUpdatePos::run, &tc3);
//	boost::thread thread4(&ThreadUpdatePos::run, &tc4);
//	thread3.join();
//	thread4.join();
127

Pierre Kraemer's avatar
Pierre Kraemer committed
128
129
130
131
	for (unsigned int i = 0; i < agents_.size(); ++i)
	{
		Dart oldFace = agents_[i]->part_.d;
		agents_[i]->update();
Pierre Kraemer's avatar
Pierre Kraemer committed
132
133
134
135
//		if(envMap_.map.getEmbedding(oldFace, FACE_ORBIT) != envMap_.map.getEmbedding(agents_[i]->part_.d, FACE_ORBIT))
		if(agents_[i]->part_.crossCell != CGoGN::Algo::MovingObjects::NO_CROSS)
//		switch(agents_[i]->part_.crossCell)
		{
Pierre Kraemer's avatar
Pierre Kraemer committed
136
//			case CGoGN::Algo::MovingObjects::CROSS_EDGE :
Pierre Kraemer's avatar
Pierre Kraemer committed
137
138
139
//				envMap_.agentChangeFaceThroughEdge(agents_[i]);
//				envMap_.addCoarsenCandidate(oldFace);
//				envMap_.addRefineCandidate(agents_[i]->part_.d);
Pierre Kraemer's avatar
Pierre Kraemer committed
140
141
142
//				break;
//			case CGoGN::Algo::MovingObjects::CROSS_OTHER :
				envMap_.agentChangeFace(agents_[i], oldFace);
143
//				envMap_.agentChangeFace(agents_[i], oldDarts[i]);
Pierre Kraemer's avatar
Pierre Kraemer committed
144
//				break;
Pierre Kraemer's avatar
Pierre Kraemer committed
145
146
		}

Pierre Kraemer's avatar
Pierre Kraemer committed
147
148
	}

Pierre Kraemer's avatar
Pierre Kraemer committed
149
	envMap_.updateMap();
Pierre Kraemer's avatar
Pierre Kraemer committed
150
151

	globalTime_ += timeStep_;
Pierre Kraemer's avatar
Pierre Kraemer committed
152
153
154
155
156

	if(globalTime_ - int(globalTime_) == 0.0f && int(globalTime_) % 500 == 0)
	{
		swapAgentsGoals();
	}
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
166
167
{
	/*
	* Add agents, specifying their start position, and store their goals on the
	* opposite side of the environment.
	*/
	Dart d = envMap_.map.begin();
	DartMarkerStore filled(envMap_.map);

168
169
	unsigned int nbx = 5;
	unsigned int nby = 5;
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
182
183
	{
		bool found = false;
		VEC3 pos;
		Dart dCell;
		while(!found && d != envMap_.map.end())
		{
			if(!filled.isMarked(d) && !envMap_.buildingMark.isMarked(d))
			{
				filled.markOrbit(FACE_ORBIT, d);
				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
202
				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);
					agents_.back()->goals_.push_back(-1.0f * posagent);
Pierre Kraemer's avatar
Pierre Kraemer committed
203
					agents_.back()->curGoal_ = 1;
Pierre Kraemer's avatar
Pierre Kraemer committed
204
205
206
207
208
209
210
211
212
				}
			}
		}
	}
	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
213
		envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d);
Pierre Kraemer's avatar
Pierre Kraemer committed
214
}
215

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

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

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

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

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

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

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

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

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

Pierre Kraemer's avatar
Pierre Kraemer committed
270
271
272
273
274
275
276
277
278
279
280
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
281
282
	while ( myfile.good() )
	{
Pierre Kraemer's avatar
Pierre Kraemer committed
283
284
		getline(myfile,line);

Pierre Kraemer's avatar
Pierre Kraemer committed
285
286
		if(line.size() > 1)
		{
Pierre Kraemer's avatar
Pierre Kraemer committed
287
288
289
			std::istringstream iss(line);
			float x,y,z;

Pierre Kraemer's avatar
Pierre Kraemer committed
290
291
292
293
294
			iss >> x;
			iss >> y;
			iss >> z;

			VEC3 pos(x, y, z);
Pierre Kraemer's avatar
Pierre Kraemer committed
295
296
297
298
299
			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
300
			agents_.back()->curGoal_ = 1;
Pierre Kraemer's avatar
Pierre Kraemer committed
301
302
303
		}
	}

Pierre Kraemer's avatar
Pierre Kraemer committed
304
305
	swapAgentsGoals();

Pierre Kraemer's avatar
Pierre Kraemer committed
306
	for (unsigned int i = 0; i < agents_.size(); ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
307
		envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d);
Pierre Kraemer's avatar
Pierre Kraemer committed
308
309
310
311
312
313
314
315

	myfile.close();
	return true;
}

bool Simulator::exportAgents(std::string filename)
{
	std::ofstream out(filename.c_str(), std::ios::out);
316
317
	if (!out.good())
	{
Pierre Kraemer's avatar
Pierre Kraemer committed
318
319
320
321
		std::cerr << "(export) Unable to open file " << filename << std::endl;
		return false;
	}

322
	for (unsigned int i = 0; i < agents_.size(); ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
323
324
325
326
327
328
329
330
		out << agents_[i]->part_.m_position << std::endl;

	out.close();
	return true;
}

void Simulator::swapAgentsGoals()
{
Pierre Kraemer's avatar
Pierre Kraemer committed
331
332
333
334
335
	unsigned int nbAgents = agents_.size();
	for(unsigned int i = 0; i < nbAgents; ++i)
	{
		unsigned int r = rand() % nbAgents;

Pierre Kraemer's avatar
Pierre Kraemer committed
336
337
		if(agents_[i]->goals_[1][0] != agents_[r]->goals_[1][0] || agents_[i]->goals_[1][1] != agents_[r]->goals_[1][1])
		{
338
339
340
341
			VEC3 g = agents_[i]->goals_[1];
			agents_[i]->goals_[1] = agents_[r]->goals_[1];
			agents_[r]->goals_[1] = g;
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
342
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
343
}
344