Line data Source code
1 : #include "UI/Simulator.hpp"
2 :
3 : #include <spdlog/spdlog.h>
4 :
5 : #include <SFML/Graphics/Color.hpp>
6 : #include <SFML/Graphics/PrimitiveType.hpp>
7 : #include <SFML/Graphics/Vertex.hpp>
8 : #include <SFML/System/Vector2.hpp>
9 : #include <SFML/Window/Event.hpp>
10 : #include <chrono>
11 : #include <filesystem>
12 : #include <limits>
13 : #include <mapbox/earcut.hpp>
14 : #include <queue>
15 : #include <stdexcept>
16 : #include <thread>
17 :
18 : #include "Dynamic/Env/Env.hpp"
19 : #include "color/rgb/rgb.hpp"
20 : #include "data/SUMO/Additionals/Additionals.hpp"
21 : #include "data/SUMO/Additionals/Poi.hpp"
22 : #include "data/SUMO/Additionals/Poly.hpp"
23 : #include "data/SUMO/Network.hpp"
24 : #include "data/SUMO/SUMO.hpp"
25 : #include "rapidxml_utils.hpp"
26 : #include "utils/string.hpp"
27 :
28 : using namespace std;
29 : using namespace UI;
30 :
31 : using namespace rapidxml;
32 :
33 : using namespace utils::stringify;
34 :
35 : using namespace std::chrono_literals;
36 :
37 : const SUMO::Length Simulator::VEHICLE_LENGTH = Dynamic::Env::Vehicle::LENGTH - 1.0;
38 :
39 0 : Simulator::Simulator(filesystem::path configFilePath):
40 : delay(0ms),
41 : delayDelta(10ms),
42 0 : FPS_GOAL(120) {
43 0 : file<> configFile(configFilePath.c_str());
44 0 : xml_document<> doc;
45 0 : doc.parse<0>(configFile.data());
46 :
47 0 : const xml_node<> &configuration = *doc.first_node("configuration");
48 :
49 0 : const xml_node<> *input = configuration.first_node("input");
50 0 : if(input) {
51 0 : filesystem::path netFilePath = configFilePath.parent_path() / input->first_node("net-file")->first_attribute("value")->value();
52 :
53 0 : network = SUMO::Network::loadFromFile(netFilePath);
54 :
55 0 : const xml_node<> *netstateEl = input->first_node("netstate-file");
56 0 : if(netstateEl) {
57 0 : filesystem::path netstateFilePath = configFilePath.parent_path() / netstateEl->first_attribute("value")->value();
58 :
59 0 : netState.emplace(netstateFilePath, ios_base::in);
60 : }
61 :
62 0 : const xml_node<> *additionalEl = input->first_node("additional-files");
63 0 : if(additionalEl) {
64 0 : string additionalFiles = additionalEl->first_attribute("value")->value();
65 0 : for(const string_view &additionalStr: utils::split(additionalFiles, ",")) {
66 0 : filesystem::path additionalPath = configFilePath.parent_path() / additionalStr;
67 :
68 0 : additionals = SUMO::Additionals::loadFromFile(additionalPath);
69 : }
70 : }
71 : }
72 :
73 0 : const xml_node<> &processing = *configuration.first_node("processing");
74 :
75 0 : const xml_node<> *beginEl = processing.first_node("begin");
76 0 : if(beginEl) {
77 0 : begin = stringify<SUMO::Time>::fromString(beginEl->first_attribute("value")->value());
78 : }
79 :
80 0 : const xml_node<> *endEl = processing.first_node("end");
81 0 : if(endEl) {
82 0 : end = stringify<SUMO::Time>::fromString(endEl->first_attribute("value")->value());
83 : }
84 :
85 0 : const xml_node<> *output = configuration.first_node("output");
86 0 : if(output) {
87 0 : const xml_node<> *screenCapturesEl = output->first_node("screen-captures");
88 0 : if(screenCapturesEl) {
89 0 : screenCaptures = stringify<bool>::fromString(screenCapturesEl->first_attribute("value")->value());
90 : }
91 : }
92 :
93 0 : const xml_node<> *gui_only = configuration.first_node("gui_only");
94 0 : if(gui_only) {
95 0 : const xml_node<> *zOrderEl = gui_only->first_node("z-order");
96 0 : if(zOrderEl) {
97 0 : zOrder = stringify<bool>::fromString(zOrderEl->first_attribute("value")->value());
98 : }
99 :
100 0 : const xml_node<> *backgroundColorEl = gui_only->first_node("background-color");
101 0 : if(backgroundColorEl) {
102 0 : backgroundColor = stringify<color::rgb<float>>::fromString(backgroundColorEl->first_attribute("value")->value());
103 : }
104 : }
105 :
106 0 : loadNetworkGUI();
107 0 : loadBackground();
108 0 : }
109 :
110 0 : sf::Color Simulator::generateNewVehicleColor() const {
111 0 : uniform_real_distribution<float> hDist(0.0, 360.0);
112 0 : uniform_real_distribution<float> sDist(25.0, 100.0);
113 0 : uniform_real_distribution<float> vDist(50.0, 100.0);
114 :
115 0 : float h = hDist(gen);
116 0 : float s = sDist(gen);
117 0 : float v = vDist(gen);
118 :
119 0 : color::hsv<float> colorHSV({h, s, v});
120 0 : color::rgb<float> colorRGB;
121 0 : colorRGB = colorHSV;
122 :
123 0 : sf::Uint8 r = sf::Uint8(255.0 * color::get::red(colorRGB));
124 0 : sf::Uint8 g = sf::Uint8(255.0 * color::get::green(colorRGB));
125 0 : sf::Uint8 b = sf::Uint8(255.0 * color::get::blue(colorRGB));
126 :
127 0 : return sf::Color(r, g, b);
128 : }
129 :
130 : template<class Color>
131 0 : sf::Color color2SFML(const Color &c) {
132 0 : sf::Uint8 r = (sf::Uint8)(255.0 * color::get::red(c));
133 0 : sf::Uint8 g = (sf::Uint8)(255.0 * color::get::green(c));
134 0 : sf::Uint8 b = (sf::Uint8)(255.0 * color::get::blue(c));
135 :
136 0 : return sf::Color(r, g, b);
137 : }
138 :
139 0 : color::hsv<float> Simulator::edgeColorHeight(SUMO::Length height) const {
140 0 : color::hsv<float> c;
141 0 : c = EDGE_COLOR;
142 :
143 0 : SUMO::Length z = height / 8.0;
144 0 : z = max(z, 0.0);
145 :
146 0 : float v = (float)(100.0 * (1.0 - exp(log(1 - 0.25) * z)));
147 :
148 0 : c.set(2, v);
149 :
150 0 : return c;
151 : }
152 :
153 0 : void Simulator::loadEdges() {
154 0 : for(const SUMO::Network::Edge &edge: network->getEdges()) {
155 0 : if(edge.function == SUMO::Network::Edge::Function::INTERNAL) continue;
156 :
157 0 : const SUMO::Length fromZ = edge.from->get().pos.Z;
158 0 : const SUMO::Length toZ = edge.to->get().pos.Z;
159 :
160 0 : for(const SUMO::Network::Edge::Lane &lane: edge.lanes) {
161 0 : const SUMO::Shape &shape = lane.shape;
162 0 : assert(shape.size() >= 2);
163 0 : SUMO::Length L = 0.0;
164 0 : for(
165 0 : auto it1 = shape.begin(),
166 0 : it2 = ++shape.begin();
167 0 : it2 != shape.end();
168 0 : ++it1, ++it2
169 : ) {
170 0 : SUMO::Coord u = *it1 - offset;
171 0 : SUMO::Coord v = *it2 - offset;
172 :
173 0 : Vector2 uv = v - u;
174 0 : Vector2 uvPerp(uv.Y, -uv.X);
175 0 : uvPerp /= Vector2::Magnitude(uvPerp);
176 :
177 0 : Vector2 u1Coord = u + uvPerp * LANE_WIDTH / 2;
178 0 : Vector2 u2Coord = u - uvPerp * LANE_WIDTH / 2;
179 :
180 0 : Vector2 v1Coord = v + uvPerp * LANE_WIDTH / 2;
181 0 : Vector2 v2Coord = v - uvPerp * LANE_WIDTH / 2;
182 :
183 0 : sf::Vector2f u1((float)u1Coord.X, (float)-u1Coord.Y);
184 0 : sf::Vector2f u2((float)u2Coord.X, (float)-u2Coord.Y);
185 0 : sf::Vector2f v1((float)v1Coord.X, (float)-v1Coord.Y);
186 0 : sf::Vector2f v2((float)v2Coord.X, (float)-v2Coord.Y);
187 :
188 0 : SUMO::Length l1 = L;
189 0 : SUMO::Length l2 = L + Vector2::Magnitude(uv);
190 :
191 0 : double uProgress = shape.getProgress(l1);
192 0 : double vProgress = shape.getProgress(l2);
193 :
194 0 : #pragma GCC diagnostic push
195 0 : #pragma GCC diagnostic ignored "-Wfloat-equal"
196 0 : double uZ = u.Z;
197 0 : if(uZ == 0.0)
198 0 : uZ = (1.0 - uProgress) * fromZ + uProgress * toZ;
199 0 : color::hsv<float> uC = edgeColorHeight(uZ);
200 :
201 0 : double vZ = v.Z;
202 0 : if(vZ == 0.0)
203 0 : vZ = (1.0 - vProgress) * fromZ + vProgress * toZ;
204 0 : color::hsv<float> vC = edgeColorHeight(vZ);
205 0 : #pragma GCC diagnostic pop
206 :
207 : // clang-format off
208 0 : vector<sf::Vertex> vertices = {
209 0 : sf::Vertex(u1, color2SFML(uC)),
210 0 : sf::Vertex(u2, color2SFML(uC)),
211 0 : sf::Vertex(v1, color2SFML(vC)),
212 :
213 0 : sf::Vertex(u2, color2SFML(uC)),
214 0 : sf::Vertex(v1, color2SFML(vC)),
215 0 : sf::Vertex(v2, color2SFML(vC))
216 0 : };
217 : // clang-format on
218 :
219 : // Add connection between consecutive segments of a lane
220 0 : auto it3 = it2;
221 0 : ++it3;
222 0 : if(it3 != shape.end()) {
223 0 : SUMO::Coord w = *it3 - offset;
224 :
225 0 : Vector2 vw = w - v;
226 :
227 0 : Vector2 vwPerp(vw.Y, -vw.X);
228 0 : vwPerp /= Vector2::Magnitude(vwPerp);
229 :
230 0 : Vector2 v1_Coord = v + vwPerp * LANE_WIDTH / 2;
231 0 : Vector2 v2_Coord = v - vwPerp * LANE_WIDTH / 2;
232 :
233 0 : sf::Vector2f v1_((float)v1_Coord.X, (float)-v1_Coord.Y);
234 0 : sf::Vector2f v2_((float)v2_Coord.X, (float)-v2_Coord.Y);
235 :
236 0 : vertices.emplace_back(v1, color2SFML(vC));
237 0 : vertices.emplace_back(v2, color2SFML(vC));
238 0 : vertices.emplace_back(v1_, color2SFML(vC));
239 :
240 0 : vertices.emplace_back(v2, color2SFML(vC));
241 0 : vertices.emplace_back(v1_, color2SFML(vC));
242 0 : vertices.emplace_back(v2_, color2SFML(vC));
243 : }
244 :
245 0 : networkMap.emplace((uZ + vZ) / 2.0, vertices);
246 :
247 0 : L += Vector2::Magnitude(uv);
248 : }
249 :
250 0 : SUMO::Coord u = lane.shape.at(lane.shape.size() - 2) - offset;
251 0 : SUMO::Coord v = lane.shape.at(lane.shape.size() - 1) - offset;
252 :
253 0 : Vector2 uv = v - u;
254 0 : uv /= Vector2::Magnitude(uv);
255 :
256 0 : Vector2 uvPerp(uv.Y, -uv.X);
257 :
258 0 : Vector2 arrowFrontCoord = v - uv * ARROW_DIST_TO_JUNCTION;
259 0 : Vector2 arrowBackCoord = arrowFrontCoord - uv * ARROW_LENGTH;
260 0 : Vector2 arrowBack1Coord = arrowBackCoord + uvPerp * (ARROW_WIDTH / 2);
261 0 : Vector2 arrowBack2Coord = arrowBackCoord - uvPerp * (ARROW_WIDTH / 2);
262 :
263 0 : sf::Vector2f arrowFront((float)arrowFrontCoord.X, (float)-arrowFrontCoord.Y);
264 0 : sf::Vector2f arrowBack1((float)arrowBack1Coord.X, (float)-arrowBack1Coord.Y);
265 0 : sf::Vector2f arrowBack2((float)arrowBack2Coord.X, (float)-arrowBack2Coord.Y);
266 :
267 0 : #pragma GCC diagnostic push
268 0 : #pragma GCC diagnostic ignored "-Wfloat-equal"
269 0 : double vZ = v.Z;
270 0 : if(vZ == 0.0)
271 0 : vZ = toZ;
272 0 : #pragma GCC diagnostic pop
273 :
274 0 : vZ += 1e-3;
275 :
276 : // clang-format off
277 0 : vector<sf::Vertex> vertices = {
278 0 : sf::Vertex(arrowFront, ARROW_COLOR),
279 : sf::Vertex(arrowBack1, ARROW_COLOR),
280 : sf::Vertex(arrowBack2, ARROW_COLOR)
281 0 : };
282 : // clang-format on
283 :
284 0 : networkMap.emplace(vZ, vertices);
285 : }
286 : }
287 0 : }
288 :
289 0 : void Simulator::loadJunctions() {
290 0 : for(const SUMO::Network::Junction &junction: network->getJunctions()) {
291 0 : if(junction.type == SUMO::Network::Junction::Type::INTERNAL) continue;
292 :
293 0 : if(junction.shape.size() < 2) {
294 0 : throw runtime_error("Junction " + junction.id + " has less than 2 points in shape");
295 : }
296 :
297 0 : SUMO::Shape shape = junction.shape;
298 0 : for(SUMO::Coord &coord: shape)
299 0 : coord -= offset;
300 :
301 0 : using Point = std::array<double, 2>;
302 :
303 0 : vector<vector<Point>> shapes(1);
304 0 : for(size_t i = 0; i < shape.size(); ++i) {
305 0 : shapes[0].emplace_back(Point({shape.at(i).X, shape.at(i).Y}));
306 : }
307 :
308 0 : std::vector<size_t> indices = mapbox::earcut<size_t>(shapes);
309 :
310 0 : vector<sf::Vertex> junctionVertex;
311 0 : for(size_t i = 0; i < indices.size(); ++i) {
312 0 : SUMO::Coord uCoord = shape.at(indices[i]);
313 :
314 0 : sf::Vector2f u((float)uCoord.X, (float)-uCoord.Y);
315 :
316 0 : junctionVertex.emplace_back(u, JUNCTION_COLOR);
317 : }
318 :
319 0 : for(const SUMO::Network::Edge::Lane &lane: junction.intLanes) {
320 0 : for(
321 0 : auto it1 = lane.shape.begin(),
322 0 : it2 = ++lane.shape.begin();
323 0 : it2 != lane.shape.end();
324 0 : ++it1, ++it2
325 : ) {
326 0 : SUMO::Coord u = *it1 - offset;
327 0 : SUMO::Coord v = *it2 - offset;
328 :
329 0 : Vector2 uv = v - u;
330 0 : Vector2 uvPerp(uv.Y, -uv.X);
331 0 : uvPerp /= Vector2::Magnitude(uvPerp);
332 :
333 0 : Vector2 u1Coord = u + uvPerp * CONNECTION_WIDTH / 2;
334 0 : Vector2 u2Coord = u - uvPerp * CONNECTION_WIDTH / 2;
335 :
336 0 : Vector2 v1Coord = v + uvPerp * CONNECTION_WIDTH / 2;
337 0 : Vector2 v2Coord = v - uvPerp * CONNECTION_WIDTH / 2;
338 :
339 0 : sf::Vector2f u1((float)u1Coord.X, (float)-u1Coord.Y);
340 0 : sf::Vector2f u2((float)u2Coord.X, (float)-u2Coord.Y);
341 0 : sf::Vector2f v1((float)v1Coord.X, (float)-v1Coord.Y);
342 0 : sf::Vector2f v2((float)v2Coord.X, (float)-v2Coord.Y);
343 :
344 0 : junctionVertex.emplace_back(u1, CONNECTION_COLOR);
345 0 : junctionVertex.emplace_back(u2, CONNECTION_COLOR);
346 0 : junctionVertex.emplace_back(v1, CONNECTION_COLOR);
347 :
348 0 : junctionVertex.emplace_back(u2, CONNECTION_COLOR);
349 0 : junctionVertex.emplace_back(v1, CONNECTION_COLOR);
350 0 : junctionVertex.emplace_back(v2, CONNECTION_COLOR);
351 : }
352 : }
353 :
354 0 : networkMap.emplace(junction.pos.Z, junctionVertex);
355 : }
356 0 : }
357 :
358 0 : void Simulator::loadBackground() {
359 0 : backgroundVertices.clear();
360 :
361 0 : map<float, std::vector<sf::Vertex>> verticesMap;
362 :
363 0 : for(const auto &additional: additionals) {
364 0 : const SUMO::Additionals::Poly *polyPtr = dynamic_cast<const SUMO::Additionals::Poly *>(additional.get());
365 0 : if(polyPtr) {
366 0 : const SUMO::Additionals::Poly &poly = *polyPtr;
367 :
368 0 : if(poly.shape.size() < 2) {
369 0 : spdlog::debug("Ignoring background poly {} since shape has size ", poly.shape.size());
370 0 : continue;
371 : }
372 :
373 0 : const float layer = (poly.layer.has_value() ? poly.layer.value() : 0.0f);
374 0 : const float lineWidth = (poly.lineWidth.has_value() ? poly.lineWidth.value() : 1.0f);
375 :
376 0 : color::rgb<float> color;
377 0 : if(poly.color.has_value())
378 0 : color = poly.color.value();
379 : else
380 0 : color = color::cmy<float>({0.0, 255.0, 0.0});
381 :
382 0 : if(poly.fill) {
383 0 : SUMO::Shape shape = poly.shape;
384 0 : for(SUMO::Coord &coord: shape)
385 0 : coord -= offset;
386 :
387 0 : using Point = std::array<double, 2>;
388 :
389 0 : vector<vector<Point>> shapes(1);
390 0 : for(size_t i = 0; i < shape.size(); ++i) {
391 0 : shapes[0].emplace_back(Point({shape.at(i).X, shape.at(i).Y}));
392 : }
393 :
394 0 : std::vector<size_t> indices = mapbox::earcut<size_t>(shapes);
395 :
396 0 : vector<sf::Vertex> vertices;
397 0 : for(size_t i = 0; i < indices.size(); ++i) {
398 0 : SUMO::Coord uCoord = shape.at(indices[i]);
399 :
400 0 : sf::Vector2f u((float)uCoord.X, (float)-uCoord.Y);
401 :
402 0 : vertices.emplace_back(u, color2SFML(color));
403 : }
404 :
405 0 : vector<sf::Vertex> &vec = verticesMap[layer];
406 0 : vec.insert(
407 0 : vec.end(),
408 : vertices.begin(),
409 : vertices.end()
410 0 : );
411 : } else {
412 0 : const SUMO::Shape &shape = poly.shape;
413 0 : for(
414 0 : auto it1 = shape.begin(),
415 0 : it2 = ++shape.begin();
416 0 : it2 != shape.end();
417 0 : ++it1, ++it2
418 : ) {
419 0 : SUMO::Coord u = *it1 - offset;
420 0 : SUMO::Coord v = *it2 - offset;
421 :
422 0 : Vector2 uv = v - u;
423 0 : Vector2 uvPerp(uv.Y, -uv.X);
424 0 : uvPerp /= Vector2::Magnitude(uvPerp);
425 :
426 0 : Vector2 u1Coord = u + uvPerp * lineWidth / 2;
427 0 : Vector2 u2Coord = u - uvPerp * lineWidth / 2;
428 :
429 0 : Vector2 v1Coord = v + uvPerp * lineWidth / 2;
430 0 : Vector2 v2Coord = v - uvPerp * lineWidth / 2;
431 :
432 0 : sf::Vector2f u1((float)u1Coord.X, (float)-u1Coord.Y);
433 0 : sf::Vector2f u2((float)u2Coord.X, (float)-u2Coord.Y);
434 0 : sf::Vector2f v1((float)v1Coord.X, (float)-v1Coord.Y);
435 0 : sf::Vector2f v2((float)v2Coord.X, (float)-v2Coord.Y);
436 :
437 : // clang-format off
438 0 : vector<sf::Vertex> vertices = {
439 0 : sf::Vertex(u1, color2SFML(color)),
440 0 : sf::Vertex(u2, color2SFML(color)),
441 0 : sf::Vertex(v1, color2SFML(color)),
442 :
443 0 : sf::Vertex(u2, color2SFML(color)),
444 0 : sf::Vertex(v1, color2SFML(color)),
445 0 : sf::Vertex(v2, color2SFML(color))
446 0 : };
447 :
448 0 : vector<sf::Vertex> &vec = verticesMap[layer];
449 0 : vec.insert(
450 0 : vec.end(),
451 : vertices.begin(),
452 : vertices.end()
453 0 : );
454 : }
455 : }
456 :
457 0 : continue;
458 : }
459 0 : const SUMO::Additionals::Poi *poiPtr = dynamic_cast<const SUMO::Additionals::Poi *>(additional.get());
460 0 : if(poiPtr) {
461 0 : const SUMO::Additionals::Poi &poi = *poiPtr;
462 :
463 0 : float layer = (poi.layer.has_value() ? poi.layer.value() : 0.0);
464 :
465 0 : const size_t NUMBER_POINTS = 20;
466 :
467 0 : SUMO::Coord uCoord = poi.posXY.value() - offset;
468 :
469 0 : color::rgb<float> color;
470 0 : if(poi.color.has_value())
471 0 : color = poi.color.value();
472 : else
473 0 : color = color::cmy<float>({0.0, 255.0, 0.0});
474 :
475 0 : sf::Vector2f u((float)uCoord.X, (float)-uCoord.Y);
476 :
477 0 : for(size_t i = 0; i < NUMBER_POINTS; ++i) {
478 0 : double angle1 = 2.0 * M_PI * (double)i / (double)NUMBER_POINTS;
479 0 : double angle2 = 2.0 * M_PI * (double)(i + 1) / (double)NUMBER_POINTS;
480 :
481 0 : SUMO::Coord v1Coord = uCoord + SUMO::Coord({cos(angle1), sin(angle1)}) * 1.0;
482 0 : SUMO::Coord v2Coord = uCoord + SUMO::Coord({cos(angle2), sin(angle2)}) * 1.0;
483 :
484 0 : sf::Vector2f v1((float)v1Coord.X, (float)-v1Coord.Y);
485 0 : sf::Vector2f v2((float)v2Coord.X, (float)-v2Coord.Y);
486 :
487 0 : vector<sf::Vertex> vertices = {
488 0 : sf::Vertex(u, color2SFML(color)),
489 0 : sf::Vertex(v1, color2SFML(color)),
490 0 : sf::Vertex(v2, color2SFML(color))};
491 :
492 0 : vector<sf::Vertex> &vec = verticesMap[layer];
493 0 : vec.insert(
494 0 : vec.end(),
495 : vertices.begin(),
496 : vertices.end()
497 0 : );
498 : }
499 :
500 0 : continue;
501 : }
502 : }
503 :
504 0 : for(const auto &[layer, vertices]: verticesMap) {
505 0 : backgroundVertices.insert(
506 0 : backgroundVertices.end(),
507 : vertices.begin(),
508 : vertices.end()
509 0 : );
510 : }
511 0 : }
512 :
513 0 : void Simulator::loadNetworkGUI() {
514 0 : offset = network->location.center();
515 :
516 0 : networkMap.clear();
517 0 : networkVertices.clear();
518 :
519 0 : loadEdges();
520 0 : loadJunctions();
521 :
522 0 : if(!zOrder) {
523 0 : for(const auto &[z, v]: networkMap) {
524 0 : networkVertices.insert(
525 0 : networkVertices.end(),
526 : v.begin(),
527 : v.end()
528 0 : );
529 : }
530 0 : networkMap.clear();
531 : }
532 0 : }
533 :
534 : double EPSILON = 1e-3;
535 :
536 0 : void Simulator::loadVehicles() {
537 0 : vehiclesMap.clear();
538 0 : vehicleVertices.clear();
539 :
540 0 : for(const auto &[edgeID, tsEdge]: timestep.edges) {
541 0 : const SUMO::Network::Edge &edge = network->getEdge(edgeID);
542 :
543 0 : const SUMO::Length fromZ = edge.from->get().pos.Z;
544 0 : const SUMO::Length toZ = edge.to->get().pos.Z;
545 :
546 0 : for(const auto &[laneID, tsLane]: tsEdge.lanes) {
547 0 : for(const auto &vehicle: tsLane.vehicles) {
548 0 : const SUMO::Network::Edge::Lane &lane = network->getEdge(edgeID).lanes.at(tsLane.index());
549 :
550 0 : double progress = vehicle.pos / lane.length;
551 0 : progress = max(0.0, min(1.0, progress));
552 :
553 0 : SUMO::Coord pos = lane.shape.locationAtProgress(progress) - offset;
554 0 : Vector2 dir = lane.shape.directionAtProgress(progress);
555 :
556 0 : Vector2 dirPerp(dir.Y, -dir.X);
557 :
558 0 : SUMO::Coord rear = pos - dir * Dynamic::Env::Vehicle::LENGTH;
559 :
560 0 : SUMO::Coord rear1 = rear + dirPerp * VEHICLE_WIDTH / 2;
561 0 : SUMO::Coord rear2 = rear - dirPerp * VEHICLE_WIDTH / 2;
562 :
563 0 : sf::Vector2f sfPos((float)pos.X, (float)-pos.Y);
564 0 : sf::Vector2f sfRear1((float)rear1.X, (float)-rear1.Y);
565 0 : sf::Vector2f sfRear2((float)rear2.X, (float)-rear2.Y);
566 :
567 0 : sf::Color c;
568 :
569 0 : if(vehicleColor.count(vehicle.id))
570 0 : c = vehicleColor.at(vehicle.id);
571 : else
572 0 : c = vehicleColor[vehicle.id] = generateNewVehicleColor();
573 :
574 0 : #pragma GCC diagnostic push
575 0 : #pragma GCC diagnostic ignored "-Wfloat-equal"
576 0 : SUMO::Length z = pos.Z;
577 0 : if(z == 0.0)
578 0 : z = (1.0 - progress) * fromZ + progress * toZ;
579 0 : #pragma GCC diagnostic pop
580 :
581 0 : z += 0.50;
582 :
583 : // clang-format off
584 0 : vector<sf::Vertex> vertices = {
585 : sf::Vertex(sfPos, c),
586 : sf::Vertex(sfRear1, c),
587 : sf::Vertex(sfRear2, c)
588 0 : };
589 : // clang-format on
590 :
591 0 : if(!zOrder) {
592 0 : vehicleVertices.insert(
593 0 : vehicleVertices.end(),
594 : vertices.begin(),
595 : vertices.end()
596 0 : );
597 : } else {
598 0 : vehiclesMap.emplace(z, vertices);
599 : }
600 : }
601 : }
602 : }
603 0 : }
604 :
605 0 : void Simulator::loadTrafficLights() {
606 0 : trafficLightsMap.clear();
607 0 : trafficLightVertices.clear();
608 :
609 0 : for(const SUMO::Network::Edge &edge: network->getEdges()) {
610 0 : if(edge.function == SUMO::Network::Edge::Function::INTERNAL) continue;
611 :
612 0 : for(const SUMO::Network::Edge::Lane &lane: edge.lanes) {
613 0 : vector<SUMO::Network::TrafficLightLogic::Phase::State> states;
614 0 : for(const SUMO::Network::Connection &connection: lane.getOutgoing()) {
615 0 : if(!connection.tl.has_value()) continue;
616 0 : states.emplace_back(connection.getTrafficLightState(timestep.time));
617 : }
618 :
619 0 : if(states.empty()) continue;
620 :
621 0 : SUMO::Shape shape = lane.getShape();
622 :
623 0 : SUMO::Coord p = shape.back() - offset;
624 0 : Vector2 dir = shape.directionAtProgress(1.0);
625 0 : dir /= Vector2::Magnitude(dir);
626 0 : SUMO::Coord dirPerp(-dir.Y, dir.X);
627 :
628 0 : SUMO::Coord pRight = p - dirPerp * LANE_WIDTH / 2;
629 :
630 0 : float TRAFFIC_LIGHT_WIDTH = LANE_WIDTH / (float)states.size();
631 :
632 0 : for(size_t i = 0; i < states.size(); ++i) {
633 0 : SUMO::Coord l = pRight + dirPerp * double(i) * TRAFFIC_LIGHT_WIDTH;
634 0 : SUMO::Coord r = pRight + dirPerp * double(i + 1) * TRAFFIC_LIGHT_WIDTH;
635 :
636 0 : SUMO::Coord p1Coord = l + dir * TRAFFIC_LIGHT_LENGTH / 2;
637 0 : SUMO::Coord p2Coord = l - dir * TRAFFIC_LIGHT_LENGTH / 2;
638 0 : SUMO::Coord p3Coord = r + dir * TRAFFIC_LIGHT_LENGTH / 2;
639 0 : SUMO::Coord p4Coord = r - dir * TRAFFIC_LIGHT_LENGTH / 2;
640 :
641 0 : sf::Vector2f p1((float)p1Coord.X, (float)-p1Coord.Y);
642 0 : sf::Vector2f p2((float)p2Coord.X, (float)-p2Coord.Y);
643 0 : sf::Vector2f p3((float)p3Coord.X, (float)-p3Coord.Y);
644 0 : sf::Vector2f p4((float)p4Coord.X, (float)-p4Coord.Y);
645 :
646 0 : sf::Color c;
647 0 : switch(states[i]) {
648 0 : case SUMO::Network::TrafficLightLogic::Phase::State::RED:
649 0 : c = sf::Color::Red;
650 0 : break;
651 0 : case SUMO::Network::TrafficLightLogic::Phase::State::YELLOW_START:
652 0 : case SUMO::Network::TrafficLightLogic::Phase::State::YELLOW_STOP:
653 0 : c = sf::Color::Yellow;
654 0 : break;
655 0 : case SUMO::Network::TrafficLightLogic::Phase::State::GREEN_PRIORITY:
656 0 : case SUMO::Network::TrafficLightLogic::Phase::State::GREEN_RIGHT:
657 0 : c = sf::Color::Green;
658 0 : break;
659 0 : case SUMO::Network::TrafficLightLogic::Phase::State::GREEN_NOPRIORITY:
660 0 : c = sf::Color(0, 128, 0);
661 0 : break;
662 0 : case SUMO::Network::TrafficLightLogic::Phase::State::OFF:
663 0 : case SUMO::Network::TrafficLightLogic::Phase::State::OFF_YIELD:
664 0 : c = sf::Color(128, 128, 128);
665 0 : break;
666 0 : default:
667 0 : throw runtime_error("Unknown traffic light state");
668 : }
669 :
670 : // clang-format off
671 0 : vector<sf::Vertex> vertices = {
672 : sf::Vertex(p1, c),
673 : sf::Vertex(p2, c),
674 : sf::Vertex(p3, c),
675 : sf::Vertex(p2, c),
676 : sf::Vertex(p3, c),
677 : sf::Vertex(p4, c),
678 0 : };
679 : // clang-format on
680 :
681 0 : if(!zOrder) {
682 0 : trafficLightVertices.insert(
683 0 : trafficLightVertices.end(),
684 : vertices.begin(),
685 : vertices.end()
686 0 : );
687 : } else {
688 0 : trafficLightsMap.emplace(numeric_limits<double>::infinity(), vertices);
689 : }
690 : }
691 : }
692 : }
693 0 : }
694 :
695 0 : void Simulator::onScroll(float delta) {
696 0 : scale *= pow(SCALE_DELTA, -delta);
697 0 : recalculateView();
698 0 : }
699 :
700 0 : void Simulator::onResize() {
701 0 : recalculateView();
702 0 : }
703 :
704 0 : void Simulator::recalculateView() {
705 0 : sf::Vector2f size((float)window->getSize().x, (float)window->getSize().y);
706 0 : view = sf::View(center, size * scale);
707 0 : }
708 :
709 : const SUMO::Time TIME_EPSILON = 1e-3;
710 :
711 0 : void Simulator::run() {
712 0 : window = make_shared<sf::RenderWindow>(sf::VideoMode(1900, 1000), "DynamiNATOR");
713 0 : window->setFramerateLimit(FPS_GOAL);
714 :
715 0 : center = sf::Vector2f(
716 0 : (float)(+network->location.center().X - offset.X),
717 0 : (float)(-network->location.center().Y + offset.Y)
718 : );
719 0 : scale = max(
720 0 : (float)network->location.size().X / (float)window->getSize().x,
721 0 : (float)network->location.size().Y / (float)window->getSize().y
722 0 : );
723 :
724 0 : recalculateView();
725 :
726 0 : window->setView(view);
727 :
728 0 : bool mustSaveScreenCapture = false;
729 :
730 0 : bool isLeftClickPressed = false;
731 0 : sf::Vector2f centerInitial;
732 0 : sf::Vector2f posMouseInitial;
733 :
734 0 : queue<clk::time_point> frames;
735 :
736 0 : clk::time_point lastFrame = clk::now();
737 0 : clk::time_point lastTimestepUpdate = clk::now();
738 :
739 0 : if(
740 0 : netState.has_value() && netState.value()
741 : ) {
742 : size_t i = 0;
743 0 : while(true) {
744 0 : netState.value() >> timestep;
745 0 : if(!begin.has_value() || timestep.time >= begin.value() - TIME_EPSILON) break;
746 :
747 0 : if(i % 100 == 0) {
748 0 : cerr << "Seeking begin " << begin.value() << ", currently at " << timestep.time << endl;
749 : }
750 :
751 0 : ++i;
752 : }
753 :
754 0 : loadVehicles();
755 0 : loadTrafficLights();
756 :
757 0 : lastTimestepUpdate = clk::now();
758 :
759 0 : mustSaveScreenCapture = true;
760 : }
761 :
762 0 : while(window->isOpen()) {
763 0 : clk::time_point now = clk::now();
764 :
765 0 : sf::Event event;
766 0 : while(window->pollEvent(event)) {
767 0 : #pragma GCC diagnostic push
768 0 : #pragma GCC diagnostic ignored "-Wswitch-enum"
769 0 : switch(event.type) {
770 0 : case sf::Event::Closed:
771 0 : window->close();
772 : break;
773 0 : case sf::Event::Resized:
774 0 : onResize();
775 : break;
776 0 : case sf::Event::MouseWheelScrolled:
777 0 : onScroll(event.mouseWheelScroll.delta);
778 : break;
779 0 : case sf::Event::MouseButtonPressed:
780 0 : switch(event.mouseButton.button) {
781 0 : case sf::Mouse::Button::Left:
782 0 : isLeftClickPressed = true;
783 0 : centerInitial = center;
784 0 : posMouseInitial = sf::Vector2f(
785 0 : (float)event.mouseButton.x,
786 0 : (float)event.mouseButton.y
787 : );
788 0 : break;
789 : default:
790 : break;
791 : }
792 : break;
793 0 : case sf::Event::MouseButtonReleased:
794 0 : switch(event.mouseButton.button) {
795 0 : case sf::Mouse::Button::Left:
796 0 : isLeftClickPressed = false;
797 0 : break;
798 : default:
799 : break;
800 : }
801 : break;
802 0 : case sf::Event::MouseMoved:
803 0 : if(isLeftClickPressed) {
804 0 : sf::Vector2f mouse_pos(
805 0 : (float)event.mouseMove.x,
806 0 : (float)event.mouseMove.y
807 0 : );
808 0 : center = centerInitial - (mouse_pos - posMouseInitial) * scale;
809 0 : recalculateView();
810 : }
811 : break;
812 : // case sf::Event::TextEntered:
813 : // switch(toupper((int)event.text.unicode)) {
814 : // case ' ':
815 : // running = !running;
816 : // break;
817 : // default:
818 : // break;
819 : // }
820 : // break;
821 0 : case sf::Event::KeyPressed:
822 0 : switch(event.key.code) {
823 0 : case sf::Keyboard::Key::Space:
824 0 : running = !running;
825 0 : break;
826 0 : case sf::Keyboard::Key::Up:
827 0 : delay += delayDelta;
828 0 : cerr << "Delay: " << delay.count() << " [ms]" << endl;
829 : break;
830 0 : case sf::Keyboard::Key::Down:
831 0 : delay -= delayDelta;
832 0 : delay = max(delay, 0ms);
833 0 : cerr << "Delay: " << delay.count() << " [ms]" << endl;
834 : break;
835 0 : case sf::Keyboard::Key::Right:
836 0 : if(netState.has_value() && netState.value()) {
837 0 : netState.value() >> timestep;
838 0 : loadVehicles();
839 0 : loadTrafficLights();
840 0 : lastTimestepUpdate = now;
841 : }
842 : break;
843 : default:
844 : break;
845 : }
846 : break;
847 : default:
848 : break;
849 : }
850 : #pragma GCC diagnostic pop
851 : }
852 :
853 : // clang-format off
854 0 : if(
855 0 : running &&
856 0 : netState.has_value() &&
857 0 : netState.value() &&
858 0 : now - lastTimestepUpdate >= delay &&
859 0 : (!end.has_value() || timestep.time < end.value())
860 : ) {
861 : // clang-format on
862 :
863 0 : netState.value() >> timestep;
864 :
865 0 : loadVehicles();
866 0 : loadTrafficLights();
867 :
868 0 : lastTimestepUpdate = now;
869 :
870 0 : mustSaveScreenCapture = true;
871 : }
872 :
873 0 : window->clear(color2SFML(backgroundColor));
874 :
875 0 : window->setView(view);
876 :
877 0 : draw();
878 :
879 0 : window->display();
880 :
881 0 : if(screenCaptures && mustSaveScreenCapture) {
882 : // Obtained from https://www.sfml-dev.org/documentation/2.6.0/classsf_1_1RenderWindow.php#a5a784b8a09bf4a8bc97ef9e0a8957c35
883 0 : sf::Vector2u windowSize = window->getSize();
884 0 : sf::Texture texture;
885 0 : texture.create(windowSize.x, windowSize.y);
886 0 : texture.update(*window);
887 0 : sf::Image screenshot = texture.copyToImage();
888 :
889 0 : string screenshotFilePath = "data/out/screenshots/amial/" + to_string(timestep.time) + ".png";
890 :
891 0 : thread([screenshot, screenshotFilePath]() -> void {
892 0 : screenshot.saveToFile(screenshotFilePath);
893 0 : }).detach();
894 :
895 0 : mustSaveScreenCapture = false;
896 : }
897 :
898 0 : frames.push(now);
899 0 : while(frames.front() < now - 1s) frames.pop();
900 :
901 0 : if(now - lastFrame >= 1s) {
902 0 : cout << frames.size() << " FPS, t=" << timestep.time << endl;
903 0 : lastFrame = now;
904 : }
905 : }
906 0 : }
907 :
908 0 : void Simulator::draw() {
909 0 : window->draw(backgroundVertices.data(), backgroundVertices.size(), sf::Triangles);
910 0 : if(!zOrder) {
911 0 : window->draw(networkVertices.data(), networkVertices.size(), sf::Triangles);
912 0 : window->draw(vehicleVertices.data(), vehicleVertices.size(), sf::Triangles);
913 0 : window->draw(trafficLightVertices.data(), trafficLightVertices.size(), sf::Triangles);
914 : } else {
915 0 : multimap<double, vector<sf::Vertex>> verticesMap;
916 0 : verticesMap.insert(networkMap.begin(), networkMap.end());
917 0 : verticesMap.insert(vehiclesMap.begin(), vehiclesMap.end());
918 0 : verticesMap.insert(trafficLightsMap.begin(), trafficLightsMap.end());
919 :
920 0 : vector<sf::Vertex> vertices;
921 0 : for(const auto &[_, v]: verticesMap) {
922 0 : vertices.insert(vertices.end(), v.begin(), v.end());
923 : }
924 :
925 0 : window->draw(vertices.data(), vertices.size(), sf::Triangles);
926 : }
927 0 : }
|
