Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2001-2024 German Aerospace Center (DLR) and others.
4 : // This program and the accompanying materials are made available under the
5 : // terms of the Eclipse Public License 2.0 which is available at
6 : // https://www.eclipse.org/legal/epl-2.0/
7 : // This Source Code may also be made available under the following Secondary
8 : // Licenses when the conditions for such availability set forth in the Eclipse
9 : // Public License 2.0 are satisfied: GNU General Public License, version 2
10 : // or later which is available at
11 : // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12 : // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13 : /****************************************************************************/
14 : /// @file Position.h
15 : /// @author Daniel Krajzewicz
16 : /// @author Jakob Erdmann
17 : /// @author Axel Wegener
18 : /// @author Michael Behrisch
19 : /// @date Sept 2002
20 : ///
21 : // A position in the 2D- or 3D-world
22 : /****************************************************************************/
23 : #pragma once
24 : #include <config.h>
25 : #include <iostream>
26 : #include <cmath>
27 :
28 : #include <config.h>
29 :
30 : // ===========================================================================
31 : // class definitions
32 : // ===========================================================================
33 : /**
34 : * @class Position
35 : * @brief A point in 2D or 3D with translation and scaling methods.
36 : */
37 : class Position {
38 : public:
39 : /// @brief default constructor
40 826354 : Position() :
41 953443339 : myX(0.0), myY(0.0), myZ(0.0) { }
42 :
43 : /// @brief Parametrised constructor (only for x-y)
44 197543816 : Position(double x, double y) :
45 28421898 : myX(x), myY(y), myZ(0) { }
46 :
47 : /// @brief Parametrised constructor
48 2372351 : Position(double x, double y, double z) :
49 19832680 : myX(x), myY(y), myZ(z) { }
50 :
51 : /// @brief Destructor
52 4533512755 : ~Position() { }
53 :
54 : /// @brief Returns the x-position
55 : inline double x() const {
56 428498334 : return myX;
57 : }
58 :
59 : /// @brief Returns the y-position
60 : inline double y() const {
61 641513498 : return myY;
62 : }
63 :
64 : /// @brief Returns the z-position
65 : inline double z() const {
66 30665759 : return myZ;
67 : }
68 :
69 : /// @brief set position x
70 : void setx(double x) {
71 1227 : myX = x;
72 2 : }
73 :
74 : /// @brief set position y
75 : void sety(double y) {
76 6768 : myY = y;
77 26 : }
78 :
79 : /// @brief set position z
80 : void setz(double z) {
81 1047240 : myZ = z;
82 6132 : }
83 :
84 : /// @brief set positions x and y
85 : void set(double x, double y) {
86 1044625 : myX = x;
87 179237 : myY = y;
88 : }
89 :
90 : /// @brief set positions x, y and z
91 : void set(double x, double y, double z) {
92 798312 : myX = x;
93 798312 : myY = y;
94 4118931 : myZ = z;
95 12549 : }
96 :
97 : /// @brief set position with another position
98 : void set(const Position& pos) {
99 480589 : myX = pos.myX;
100 480589 : myY = pos.myY;
101 480589 : myZ = pos.myZ;
102 6 : }
103 :
104 : /// @brief Multiplies position with the given value
105 : void mul(double val) {
106 50796 : myX *= val;
107 50796 : myY *= val;
108 50754 : myZ *= val;
109 38110 : }
110 :
111 : /// @brief Divides position with the given value
112 : void div(double val) {
113 : myX /= val;
114 : myY /= val;
115 : myZ /= val;
116 : }
117 :
118 : /// @brief Multiplies position with the given values
119 : void mul(double mx, double my) {
120 : myX *= mx;
121 18324 : myY *= my;
122 23 : }
123 :
124 : /// @brief Multiplies position with the given values
125 : void mul(double mx, double my, double mz) {
126 : myX *= mx;
127 : myY *= my;
128 : myZ *= mz;
129 : }
130 :
131 : /// @brief Adds the given position to this one
132 : void add(const Position& pos) {
133 7596728 : myX += pos.myX;
134 7596728 : myY += pos.myY;
135 7596728 : myZ += pos.myZ;
136 6492521 : }
137 :
138 : /// @brief Adds the given position to this one
139 : void add(double dx, double dy) {
140 2683 : myX += dx;
141 2683 : myY += dy;
142 1411 : }
143 :
144 : /// @brief Adds the given position to this one
145 : void add(double dx, double dy, double dz) {
146 7617570 : myX += dx;
147 7617570 : myY += dy;
148 7617570 : myZ += dz;
149 : }
150 :
151 : /// @brief Subtracts the given position from this one
152 : void sub(double dx, double dy) {
153 28263 : myX -= dx;
154 28263 : myY -= dy;
155 : }
156 :
157 : /// @brief Subtracts the given position from this one
158 : void sub(double dx, double dy, double dz) {
159 : myX -= dx;
160 : myY -= dy;
161 : myZ -= dz;
162 : }
163 :
164 : /// @brief Subtracts the given position from this one
165 : void sub(const Position& pos) {
166 8692367 : myX -= pos.myX;
167 8692367 : myY -= pos.myY;
168 8692367 : myZ -= pos.myZ;
169 1722988 : }
170 :
171 : /// @brief Computes the length of the given vector
172 : inline double length2D() const {
173 2816 : return sqrt(myX * myX + myY * myY);
174 : }
175 :
176 : /// @brief Normalizes the given vector
177 1592 : inline void norm2D() {
178 : const double val = length2D();
179 1592 : if (val != 0.) {
180 1592 : myX /= val;
181 1592 : myY /= val;
182 : }
183 1592 : }
184 :
185 : /// @brief output operator
186 2292546 : friend std::ostream& operator<<(std::ostream& os, const Position& p) {
187 : os << p.x() << "," << p.y();
188 2292546 : if (p.z() != double(0.0)) {
189 : os << "," << p.z();
190 : }
191 2292546 : return os;
192 : }
193 :
194 : /// @brief add operator
195 : Position operator+(const Position& p2) const {
196 21651482 : return Position(myX + p2.myX, myY + p2.myY, myZ + p2.myZ);
197 : }
198 :
199 : /// @brief sub operator
200 : Position operator-(const Position& p2) const {
201 47398285 : return Position(myX - p2.myX, myY - p2.myY, myZ - p2.myZ);
202 : }
203 :
204 : /// @brief keep the direction but modify the length of the (location) vector to length * scalar
205 : Position operator*(double scalar) const {
206 1717259209 : return Position(myX * scalar, myY * scalar, myZ * scalar);
207 : }
208 :
209 : /// @brief keep the direction but modify the length of the (location) vector to length / scalar
210 : Position operator/(double scalar) const {
211 6 : return Position(myX / scalar, myY / scalar, myZ / scalar);
212 : }
213 :
214 : /// @brief keep the direction but modify the length of the (location) vector to length + scalar
215 5344 : Position operator+(double offset) const {
216 5344 : const double length = distanceTo(Position(0, 0, 0));
217 5344 : if (length == 0) {
218 0 : return *this;
219 : }
220 5344 : const double scalar = (length + offset) / length;
221 5344 : return Position(myX * scalar, myY * scalar, myZ * scalar);
222 : }
223 :
224 : /// @brief keep the direction but modify the length of the (location) vector to length - scalar
225 : Position operator-(double offset) const {
226 : const double length = distanceTo(Position(0, 0, 0));
227 : if (length == 0) {
228 : return *this;
229 : }
230 : const double scalar = (length - offset) / length;
231 : return Position(myX * scalar, myY * scalar, myZ * scalar);
232 : }
233 :
234 : /// @brief comparation operator
235 : bool operator==(const Position& p2) const {
236 2646476093 : return myX == p2.myX && myY == p2.myY && myZ == p2.myZ;
237 : }
238 :
239 : /// @brief difference operator
240 : bool operator!=(const Position& p2) const {
241 1043851003 : return myX != p2.myX || myY != p2.myY || myZ != p2.myZ;
242 : }
243 :
244 : /// @brief lexicographical sorting for use in maps and sets
245 : bool operator<(const Position& p2) const {
246 10518 : if (myX < p2.myX) {
247 : return true;
248 6889 : } else if (myY < p2.myY) {
249 : return true;
250 : } else {
251 4887 : return myZ < p2.myZ;
252 : }
253 : }
254 :
255 : /// @brief check if two position is almost the sme as other
256 : bool almostSame(const Position& p2, double maxDiv = POSITION_EPS) const {
257 10841905 : return distanceTo(p2) < maxDiv;
258 : }
259 :
260 : /// @brief returns the euclidean distance in 3 dimension
261 3663339573 : inline double distanceTo(const Position& p2) const {
262 3663339573 : return sqrt(distanceSquaredTo(p2));
263 : }
264 :
265 : /// @brief returns the square of the distance to another position
266 : inline double distanceSquaredTo(const Position& p2) const {
267 3663339573 : return (myX - p2.myX) * (myX - p2.myX) + (myY - p2.myY) * (myY - p2.myY) + (myZ - p2.myZ) * (myZ - p2.myZ);
268 : }
269 :
270 : /// @brief returns the euclidean distance in the x-y-plane
271 : inline double distanceTo2D(const Position& p2) const {
272 342008354 : return sqrt(distanceSquaredTo2D(p2));
273 : }
274 :
275 : /// @brief returns the square of the distance to another position (Only using x and y positions)
276 : inline double distanceSquaredTo2D(const Position& p2) const {
277 566871905 : return (myX - p2.myX) * (myX - p2.myX) + (myY - p2.myY) * (myY - p2.myY);
278 : }
279 :
280 : /// @brief returns the angle in the plane of the vector pointing from here to the other position (in radians between -M_PI and M_PI)
281 : inline double angleTo2D(const Position& other) const {
282 742410083 : return atan2(other.myY - myY, other.myX - myX);
283 : }
284 :
285 : /// @brief returns the slope of the vector pointing from here to the other position (in radians between -M_PI and M_PI)
286 250433708 : inline double slopeTo2D(const Position& other) const {
287 250433708 : return atan2(other.myZ - myZ, distanceTo2D(other));
288 : }
289 :
290 : /// @brief returns the cross product between this point and the second one
291 : Position crossProduct(const Position& pos) {
292 : return Position(
293 360 : myY * pos.myZ - myZ * pos.myY,
294 360 : myZ * pos.myX - myX * pos.myZ,
295 360 : myX * pos.myY - myY * pos.myX);
296 : }
297 :
298 : /// @brief returns the dot product (scalar product) between this point and the second one
299 : inline double dotProduct(const Position& pos) const {
300 360 : return myX * pos.myX + myY * pos.myY + myZ * pos.myZ;
301 : }
302 :
303 : /// @brief rotate this position by rad around origin and return the result
304 : Position rotateAround2D(double rad, const Position& origin);
305 :
306 : /// @brief swap position X and Y
307 : void swapXY() {
308 : std::swap(myX, myY);
309 : }
310 :
311 : /// @brief check if position is NAN
312 : bool isNAN() const {
313 14367 : return (std::isnan(myX) || std::isnan(myY) || std::isnan(myZ));
314 : }
315 :
316 : /// @brief used to indicate that a position is valid
317 : static const Position INVALID;
318 :
319 : private:
320 : /// @brief The x-position
321 : double myX;
322 :
323 : /// @brief The y-position
324 : double myY;
325 :
326 : /// @brief The z-position
327 : double myZ;
328 : };
|
