simulator.cpp 6.02 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;
Pierre Kraemer's avatar
Pierre Kraemer committed
8
9
	importAgents("myAgents.pos");
//	setupScenario();
Pierre Kraemer's avatar
Pierre Kraemer committed
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
	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
27
	envMap_.map.setCurrentLevel(0);
Pierre Kraemer's avatar
Pierre Kraemer committed
28
29
30
31
32
	for (unsigned int i = 0; i < agents_.size(); ++i)
	{
		agents_[i]->computePrefVelocity();
		agents_[i]->computeNewVelocity();
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
33
	envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel());
Pierre Kraemer's avatar
Pierre Kraemer committed
34
35
36
37
38

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

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

Pierre Kraemer's avatar
Pierre Kraemer committed
62
	envMap_.updateMap();
Pierre Kraemer's avatar
Pierre Kraemer committed
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106

	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);
				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
107
					agents_.back()->curGoal_ = 1;
Pierre Kraemer's avatar
Pierre Kraemer committed
108
109
110
111
112
113
114
115
116
				}
			}
		}
	}
	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
117
		envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d);
Pierre Kraemer's avatar
Pierre Kraemer committed
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
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
}
/*
void Simulator::addPathsToAgents()
{
	FunctorIsMarked fum(envMap_.buildingMark);
	for(unsigned int i = 0; i < agents_.size(); ++i)
	{
		agents_[i]->goals_.clear();
		
		Dart dStart = agents_[i]->part_.d;
		Dart dStop = dStart;
		for(unsigned int j = 0; envMap_.buildingMark.isMarked(dStop) || j < 100; ++j)
		{
			if(dStop == envMap_.map.end())
				dStop = envMap_.map.begin();
			envMap_.map.next(dStop);
		}
		
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map, envMap_.position, dStart, dStop, fum);
		
		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));
		
		Dart dStop2 = dStop;
		for(unsigned int j = 0; envMap_.buildingMark.isMarked(dStop2) || j < 100; ++j)
		{
			if(dStop2 == envMap_.map.end())
				dStop2 = envMap_.map.begin();
			envMap_.map.next(dStop2);
		}
		
		path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map, envMap_.position, dStop, dStop2, fum);
		
		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));
			
		path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map, envMap_.position, dStop2, dStart, fum);
		
		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));
	}
}
*/
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
172
173
	while ( myfile.good() )
	{
Pierre Kraemer's avatar
Pierre Kraemer committed
174
175
		getline(myfile,line);

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

Pierre Kraemer's avatar
Pierre Kraemer committed
181
182
183
184
185
			iss >> x;
			iss >> y;
			iss >> z;

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

Pierre Kraemer's avatar
Pierre Kraemer committed
195
196
	swapAgentsGoals();

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

	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
222
223
224
225
226
227
228
229
230
	unsigned int nbAgents = agents_.size();
	for(unsigned int i = 0; i < nbAgents; ++i)
	{
		unsigned int r = rand() % nbAgents;

		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
231
232
}