Line data Source code
1 : #include "data/SUMO/SUMO.hpp"
2 :
3 : #include <algorithm>
4 : #include <cassert>
5 : #include <iostream>
6 : #include <stdexcept>
7 :
8 : #include "Vector2.hpp"
9 : #include "utils/stringify.hpp"
10 :
11 : using namespace std;
12 : using namespace SUMO;
13 : using namespace utils::stringify;
14 :
15 562603 : Coord::Coord(double x, double y, double z):
16 : Vector2(x, y),
17 8 : Z(z) {}
18 :
19 0 : Coord Coord::operator+(const Coord &rhs) const {
20 0 : return Coord(
21 0 : X + rhs.X,
22 0 : Y + rhs.Y,
23 0 : Z + rhs.Z
24 0 : );
25 : }
26 0 : Coord Coord::operator+(const Vector2 &rhs) const {
27 0 : return Coord(
28 0 : X + rhs.X,
29 0 : Y + rhs.Y,
30 0 : Z
31 0 : );
32 : }
33 :
34 286469 : Coord Coord::operator-(const Coord &rhs) const {
35 286469 : return Coord(
36 286469 : X - rhs.X,
37 286469 : Y - rhs.Y,
38 286469 : Z - rhs.Z
39 0 : );
40 : }
41 0 : Coord Coord::operator-(const Vector2 &rhs) const {
42 0 : return Coord(
43 0 : X - rhs.X,
44 0 : Y - rhs.Y,
45 0 : Z
46 0 : );
47 : }
48 :
49 0 : Coord Coord::operator*(double rhs) const {
50 0 : return Coord(
51 0 : X * rhs,
52 0 : Y * rhs,
53 0 : Z * rhs
54 0 : );
55 : }
56 :
57 0 : Coord Coord::operator/(double rhs) const {
58 0 : return Coord(
59 0 : X / rhs,
60 0 : Y / rhs,
61 0 : Z / rhs
62 0 : );
63 : }
64 :
65 0 : size_t std::hash<Coord>::operator()(const Coord &v) const {
66 0 : return hash<double>()(v.X) ^ (hash<double>()(v.Y) << 1);
67 : }
68 :
69 47145 : Shape::Shape(initializer_list<Coord> il):
70 47145 : v(il) {}
71 :
72 0 : Coord Shape::front() { return v.front(); }
73 170464 : const Coord Shape::front() const { return v.front(); }
74 :
75 0 : Coord Shape::back() { return v.back(); }
76 116005 : const Coord Shape::back() const { return v.back(); }
77 :
78 0 : Shape::iterator Shape::begin() { return v.begin(); }
79 0 : Shape::iterator Shape::end() { return v.end(); }
80 :
81 0 : Shape::const_iterator Shape::begin() const { return v.begin(); }
82 0 : Shape::const_iterator Shape::end() const { return v.end(); }
83 :
84 0 : size_t Shape::size() const { return v.size(); }
85 :
86 0 : bool Shape::empty() const { return v.empty(); }
87 :
88 0 : Coord &Shape::at(size_t i) { return v.at(i); }
89 0 : const Coord &Shape::at(size_t i) const { return v.at(i); }
90 :
91 0 : Coord &Shape::operator[](size_t i) { return v[i]; }
92 0 : const Coord &Shape::operator[](size_t i) const { return v[i]; }
93 :
94 0 : void Shape::computeLengths() const {
95 0 : lengths.resize(v.size());
96 : Length l = 0;
97 0 : for(size_t i = 0; i < v.size() - 1; ++i) {
98 0 : lengths[i] = l;
99 0 : l += Vector2::Magnitude(v[i + 1] - v[i]);
100 : }
101 0 : lengths.at(v.size() - 1) = l;
102 0 : len = l;
103 0 : }
104 :
105 0 : Coord Shape::locationAtProgress(double progress) const {
106 0 : if(lengths.empty()) computeLengths();
107 :
108 0 : Length l = len.value() * progress;
109 :
110 0 : l = max(0.0, min(len.value(), l));
111 :
112 0 : ssize_t pos = upper_bound(lengths.begin(), lengths.end(), l) - lengths.begin() - 1;
113 0 : assert(pos >= 0);
114 :
115 0 : size_t i = (size_t)pos;
116 0 : assert(i < v.size());
117 :
118 0 : Length lInSegment = l - lengths[i];
119 :
120 0 : assert(lInSegment >= 0);
121 :
122 0 : if(progress >= 1.0)
123 0 : return v.back();
124 :
125 0 : assert(i < v.size() - 1);
126 :
127 0 : double progressInSegment = lInSegment / Vector2::Magnitude(v.at(i + 1) - v.at(i));
128 :
129 0 : return v.at(i) * (1.0 - progressInSegment) + v.at(i + 1) * progressInSegment;
130 : }
131 :
132 0 : Vector2 Shape::directionAtProgress(double progress) const {
133 0 : if(lengths.empty()) computeLengths();
134 :
135 0 : Length l = len.value() * progress;
136 :
137 0 : l = max(0.0, min(len.value(), l));
138 :
139 0 : ssize_t pos = upper_bound(lengths.begin(), lengths.end(), l) - lengths.begin() - 1;
140 :
141 0 : pos = min(pos, (ssize_t)v.size() - 2);
142 :
143 0 : assert(pos >= 0);
144 :
145 0 : size_t i = (size_t)pos;
146 :
147 0 : assert(i < v.size() - 1);
148 :
149 0 : Vector2 dir = v.at(i + 1) - v.at(i);
150 0 : dir /= Vector2::Magnitude(dir);
151 :
152 0 : return dir;
153 : }
154 :
155 0 : Length Shape::length() const {
156 0 : if(lengths.empty()) computeLengths();
157 :
158 0 : return *len;
159 : }
160 :
161 0 : double Shape::getProgress(SUMO::Length l) const {
162 0 : return l / length();
163 : }
164 :
165 267639 : Coord stringify<Coord>::fromString(const string &s) {
166 267639 : string str = s;
167 :
168 267639 : size_t idx;
169 :
170 267639 : idx = str.find(',');
171 267639 : double x = stringify<double>::fromString(str.substr(0, idx).c_str());
172 267639 : str = str.substr(idx + 1);
173 :
174 267639 : idx = str.find(',');
175 267639 : double y = stringify<double>::fromString(str.substr(0, idx).c_str());
176 267639 : str = (idx == string::npos ? "" : str.substr(idx + 1));
177 :
178 267639 : double z = 0.0;
179 267639 : if(!str.empty()) {
180 0 : z = stringify<double>::fromString(str.c_str());
181 : }
182 :
183 369191 : return Coord(x, y, z);
184 : }
185 :
186 0 : string stringify<Coord>::toString(const Coord &t) {
187 0 : char s[256];
188 0 : sprintf(s, "%lf,%lf", t.X, t.Y);
189 0 : return string(s);
190 : }
191 :
192 8 : pair<Coord, Coord> stringify<pair<Coord, Coord>>::fromString(const string &s) {
193 8 : string sCopy = s;
194 16 : vector<double> numbers;
195 56 : while(true) {
196 32 : size_t idx = sCopy.find(',');
197 32 : if(idx == string::npos) {
198 8 : numbers.push_back(stringify<double>::fromString(sCopy));
199 8 : break;
200 : }
201 :
202 24 : string sNumber = sCopy.substr(0, idx - 1);
203 24 : sCopy = sCopy.substr(idx + 1);
204 :
205 24 : numbers.push_back(stringify<double>::fromString(sNumber));
206 24 : }
207 :
208 8 : assert(numbers.size() == 4);
209 :
210 8 : return pair<Coord, Coord>(
211 8 : Coord(numbers.at(0), numbers.at(1)),
212 8 : Coord(numbers.at(2), numbers.at(3))
213 16 : );
214 : }
215 :
216 64478 : Shape stringify<Shape>::fromString(const std::string &s) {
217 64478 : vector<Coord> v = stringify<vector<Coord>>::fromString(s);
218 128956 : return Shape(v.begin(), v.end());
219 : }
|
