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

3
Simulator::Simulator() : globalTime_(0.0f), timeStep_(0.05f)
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;
8
9
10
//	importAgents("myAgents.pos");
	setupScenario();
//	addPathsToAgents();
Pierre Kraemer's avatar
Pierre Kraemer committed
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
	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];
}

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

Pierre Kraemer's avatar
Pierre Kraemer committed
28
	envMap_.map.setCurrentLevel(0);
Pierre Kraemer's avatar
Pierre Kraemer committed
29
30
31
32
33
	for (unsigned int i = 0; i < agents_.size(); ++i)
	{
		agents_[i]->computePrefVelocity();
		agents_[i]->computeNewVelocity();
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
34
	envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel());
Pierre Kraemer's avatar
Pierre Kraemer committed
35
36
37
38
39

	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
40
41
42
43
//		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
44
//			case CGoGN::Algo::MovingObjects::CROSS_EDGE :
Pierre Kraemer's avatar
Pierre Kraemer committed
45
46
47
//				envMap_.agentChangeFaceThroughEdge(agents_[i]);
//				envMap_.addCoarsenCandidate(oldFace);
//				envMap_.addRefineCandidate(agents_[i]->part_.d);
Pierre Kraemer's avatar
Pierre Kraemer committed
48
49
50
//				break;
//			case CGoGN::Algo::MovingObjects::CROSS_OTHER :
				envMap_.agentChangeFace(agents_[i], oldFace);
Pierre Kraemer's avatar
Pierre Kraemer committed
51
52
				envMap_.addCoarsenCandidate(oldFace);
				envMap_.addRefineCandidate(agents_[i]->part_.d);
Pierre Kraemer's avatar
Pierre Kraemer committed
53
//				break;
Pierre Kraemer's avatar
Pierre Kraemer committed
54
55
56
		}

		envMap_.map.setCurrentLevel(0);
Pierre Kraemer's avatar
Pierre Kraemer committed
57
		agents_[i]->updateObstacleNeighbors();
Pierre Kraemer's avatar
Pierre Kraemer committed
58
59
		envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel());
		
Pierre Kraemer's avatar
Pierre Kraemer committed
60
61
62
		agents_[i]->updateAgentNeighbors();
	}

Pierre Kraemer's avatar
Pierre Kraemer committed
63
	envMap_.updateMap();
Pierre Kraemer's avatar
Pierre Kraemer committed
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87

	globalTime_ += timeStep_;
}

void Simulator::setupScenario()
{
	/*
	* 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);

	for (unsigned int i = 0; i < 2000 && d != envMap_.map.end(); ++i)
	{
		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);
88
				pos[2] = 0;
Pierre Kraemer's avatar
Pierre Kraemer committed
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
				dCell = d;
				found = true;
			}
			envMap_.map.next(d);
		}

		if(found)
		{
			unsigned int nbx = 3;
			unsigned int nby = 3;
			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
109
					agents_.back()->curGoal_ = 1;
Pierre Kraemer's avatar
Pierre Kraemer committed
110
111
112
113
114
115
116
117
118
				}
			}
		}
	}
	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
119
		envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d);
Pierre Kraemer's avatar
Pierre Kraemer committed
120
}
121

Pierre Kraemer's avatar
Pierre Kraemer committed
122
123
void Simulator::addPathsToAgents()
{
124
	unsigned int dartDistForPath=50;
Pierre Kraemer's avatar
Pierre Kraemer committed
125
126
127
	for(unsigned int i = 0; i < agents_.size(); ++i)
	{
		agents_[i]->goals_.clear();
128

Pierre Kraemer's avatar
Pierre Kraemer committed
129
130
		Dart dStart = agents_[i]->part_.d;
		Dart dStop = dStart;
131
		for(unsigned int j = 0; envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath; ++j)
Pierre Kraemer's avatar
Pierre Kraemer committed
132
133
134
135
136
		{
			if(dStop == envMap_.map.end())
				dStop = envMap_.map.begin();
			envMap_.map.next(dStop);
		}
137
138
139

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

Pierre Kraemer's avatar
Pierre Kraemer committed
140
141
		for(std::vector<Dart>::iterator it = path.begin(); it!=path.end();++it)
			agents_[i]->goals_.push_back(Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position));
142

Pierre Kraemer's avatar
Pierre Kraemer committed
143
		Dart dStop2 = dStop;
144
		for(unsigned int j = 0; envMap_.buildingMark.isMarked(dStop2) || j < dartDistForPath; ++j)
Pierre Kraemer's avatar
Pierre Kraemer committed
145
146
147
148
149
		{
			if(dStop2 == envMap_.map.end())
				dStop2 = envMap_.map.begin();
			envMap_.map.next(dStop2);
		}
150
151
152

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

Pierre Kraemer's avatar
Pierre Kraemer committed
153
154
		for(std::vector<Dart>::iterator it = path.begin(); it!=path.end();++it)
			agents_[i]->goals_.push_back(Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position));
155
156
157

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

Pierre Kraemer's avatar
Pierre Kraemer committed
158
159
160
161
		for(std::vector<Dart>::iterator it = path.begin(); it!=path.end();++it)
			agents_[i]->goals_.push_back(Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position));
	}
}
162
163


Pierre Kraemer's avatar
Pierre Kraemer committed
164
165
166
167
168
169
170
171
172
173
174
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
175
176
	while ( myfile.good() )
	{
Pierre Kraemer's avatar
Pierre Kraemer committed
177
178
		getline(myfile,line);

Pierre Kraemer's avatar
Pierre Kraemer committed
179
180
		if(line.size() > 1)
		{
Pierre Kraemer's avatar
Pierre Kraemer committed
181
182
183
			std::istringstream iss(line);
			float x,y,z;

Pierre Kraemer's avatar
Pierre Kraemer committed
184
185
186
187
188
			iss >> x;
			iss >> y;
			iss >> z;

			VEC3 pos(x, y, z);
Pierre Kraemer's avatar
Pierre Kraemer committed
189
190
191
192
193
			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
194
			agents_.back()->curGoal_ = 1;
Pierre Kraemer's avatar
Pierre Kraemer committed
195
196
197
		}
	}

Pierre Kraemer's avatar
Pierre Kraemer committed
198
199
	swapAgentsGoals();

Pierre Kraemer's avatar
Pierre Kraemer committed
200
	for (unsigned int i = 0; i < agents_.size(); ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
201
		envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d);
Pierre Kraemer's avatar
Pierre Kraemer committed
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224

	myfile.close();
	return true;
}

bool Simulator::exportAgents(std::string filename)
{
	std::ofstream out(filename.c_str(), std::ios::out);
	if (!out.good()) {
		std::cerr << "(export) Unable to open file " << filename << std::endl;
		return false;
	}

	for (unsigned int i = 0; i < agents_.size(); ++i) {
		out << agents_[i]->part_.m_position << std::endl;
	}

	out.close();
	return true;
}

void Simulator::swapAgentsGoals()
{
Pierre Kraemer's avatar
Pierre Kraemer committed
225
226
227
228
229
	unsigned int nbAgents = agents_.size();
	for(unsigned int i = 0; i < nbAgents; ++i)
	{
		unsigned int r = rand() % nbAgents;

230
231
232
233
234
		if(agents_[i]->goals_[1][0]!=agents_[r]->goals_[1][0] || agents_[i]->goals_[1][1]!=agents_[r]->goals_[1][1]) {
			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
235
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
236
237
}