Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2001-2025 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 843962052 : Position() :
41 843947726 : myX(0.0), myY(0.0), myZ(0.0) { }
42 :
43 : /// @brief Parametrised constructor (only for x-y)
44 337615014 : Position(double x, double y) :
45 36438027 : myX(x), myY(y), myZ(0) { }
46 :
47 : /// @brief Parametrised constructor
48 1667785910 : Position(double x, double y, double z) :
49 26724095 : myX(x), myY(y), myZ(z) { }
50 :
51 : /// @brief Returns the x-position
52 : inline double x() const {
53 433767495 : return myX;
54 : }
55 :
56 : /// @brief Returns the y-position
57 : inline double y() const {
58 661888492 : return myY;
59 : }
60 :
61 : /// @brief Returns the z-position
62 : inline double z() const {
63 34261202 : return myZ;
64 : }
65 :
66 : /// @brief set position x
67 : void setx(double x) {
68 1668 : myX = x;
69 2 : }
70 :
71 : /// @brief set position y
72 : void sety(double y) {
73 7039 : myY = y;
74 1625 : }
75 :
76 : /// @brief set position z
77 : void setz(double z) {
78 741454 : myZ = z;
79 5154 : }
80 :
81 : /// @brief set positions x and y
82 : void set(double x, double y) {
83 860120 : myX = x;
84 157474 : myY = y;
85 : }
86 :
87 : /// @brief set positions x, y and z
88 : void set(double x, double y, double z) {
89 663992 : myX = x;
90 663992 : myY = y;
91 3293945 : myZ = z;
92 10410 : }
93 :
94 : /// @brief set position with another position
95 : void set(const Position& pos) {
96 520345 : myX = pos.myX;
97 520345 : myY = pos.myY;
98 520345 : myZ = pos.myZ;
99 5 : }
100 :
101 : /// @brief Multiplies position with the given value
102 : void mul(double val) {
103 39703 : myX *= val;
104 39703 : myY *= val;
105 39660 : myZ *= val;
106 27634 : }
107 :
108 : /// @brief Divides position with the given value
109 : void div(double val) {
110 : myX /= val;
111 : myY /= val;
112 : myZ /= val;
113 : }
114 :
115 : /// @brief Multiplies position with the given values
116 : void mul(double mx, double my) {
117 : myX *= mx;
118 18234 : myY *= my;
119 23 : }
120 :
121 : /// @brief Multiplies position with the given values
122 : void mul(double mx, double my, double mz) {
123 : myX *= mx;
124 : myY *= my;
125 : myZ *= mz;
126 : }
127 :
128 : /// @brief Adds the given position to this one
129 : void add(const Position& pos) {
130 9862835 : myX += pos.myX;
131 9862835 : myY += pos.myY;
132 9862835 : myZ += pos.myZ;
133 8977768 : }
134 :
135 : /// @brief Adds the given position to this one
136 : void add(double dx, double dy) {
137 2255 : myX += dx;
138 2255 : myY += dy;
139 944 : }
140 :
141 : /// @brief Adds the given position to this one
142 : void add(double dx, double dy, double dz) {
143 5294832 : myX += dx;
144 5294832 : myY += dy;
145 5294832 : myZ += dz;
146 : }
147 :
148 : /// @brief Subtracts the given position from this one
149 : void sub(double dx, double dy) {
150 22381 : myX -= dx;
151 22381 : myY -= dy;
152 : }
153 :
154 : /// @brief Subtracts the given position from this one
155 : void sub(double dx, double dy, double dz) {
156 : myX -= dx;
157 : myY -= dy;
158 : myZ -= dz;
159 : }
160 :
161 : /// @brief Subtracts the given position from this one
162 : void sub(const Position& pos) {
163 11400395 : myX -= pos.myX;
164 11400395 : myY -= pos.myY;
165 11400395 : myZ -= pos.myZ;
166 5589511 : }
167 :
168 : /// @brief Computes the length of the given vector
169 : inline double length() const {
170 13016 : return sqrt(myX * myX + myY * myY + myZ * myZ);
171 : }
172 :
173 : /// @brief Computes the length of the given vector neglecting the z coordinate
174 : inline double length2D() const {
175 2816 : return sqrt(myX * myX + myY * myY);
176 : }
177 :
178 : /// @brief Normalizes the given vector
179 1592 : inline void norm2D() {
180 : const double val = length2D();
181 1592 : if (val != 0.) {
182 1592 : myX /= val;
183 1592 : myY /= val;
184 : }
185 1592 : }
186 :
187 : /// @brief output operator
188 1837743 : friend std::ostream& operator<<(std::ostream& os, const Position& p) {
189 : os << p.x() << "," << p.y();
190 1837743 : if (p.z() != double(0.0)) {
191 : os << "," << p.z();
192 : }
193 1837743 : return os;
194 : }
195 :
196 : /// @brief add operator
197 : Position operator+(const Position& p2) const {
198 28118468 : return Position(myX + p2.myX, myY + p2.myY, myZ + p2.myZ);
199 : }
200 :
201 : /// @brief sub operator
202 : Position operator-(const Position& p2) const {
203 57138127 : return Position(myX - p2.myX, myY - p2.myY, myZ - p2.myZ);
204 : }
205 :
206 : /// @brief keep the direction but modify the length of the (location) vector to length * scalar
207 : Position operator*(double scalar) const {
208 1604026466 : return Position(myX * scalar, myY * scalar, myZ * scalar);
209 : }
210 :
211 : /// @brief keep the direction but modify the length of the (location) vector to length / scalar
212 : Position operator/(double scalar) const {
213 208 : return Position(myX / scalar, myY / scalar, myZ / scalar);
214 : }
215 :
216 : /// @brief keep the direction but modify the length of the (location) vector to length + scalar
217 5249 : Position operator+(double offset) const {
218 5249 : const double length = distanceTo(Position(0, 0, 0));
219 5249 : if (length == 0) {
220 0 : return *this;
221 : }
222 5249 : const double scalar = (length + offset) / length;
223 5249 : return Position(myX * scalar, myY * scalar, myZ * scalar);
224 : }
225 :
226 : /// @brief keep the direction but modify the length of the (location) vector to length - scalar
227 : Position operator-(double offset) const {
228 : const double length = distanceTo(Position(0, 0, 0));
229 : if (length == 0) {
230 : return *this;
231 : }
232 : const double scalar = (length - offset) / length;
233 : return Position(myX * scalar, myY * scalar, myZ * scalar);
234 : }
235 :
236 : /// @brief comparation operator
237 : bool operator==(const Position& p2) const {
238 2416338683 : return myX == p2.myX && myY == p2.myY && myZ == p2.myZ;
239 : }
240 :
241 : /// @brief difference operator
242 : bool operator!=(const Position& p2) const {
243 956384933 : return myX != p2.myX || myY != p2.myY || myZ != p2.myZ;
244 : }
245 :
246 : /// @brief lexicographical sorting for use in maps and sets
247 : bool operator<(const Position& p2) const {
248 31574 : if (myX != p2.myX) {
249 23310 : return myX < p2.myX;
250 : }
251 8264 : if (myY != p2.myY) {
252 30 : return myY < p2.myY;
253 : }
254 8234 : return myZ < p2.myZ;
255 : }
256 :
257 : /// @brief check whether the other position has a euclidean distance of less than maxDiv
258 : bool almostSame(const Position& p2, double maxDiv = POSITION_EPS) const {
259 8744921 : return distanceTo(p2) < maxDiv;
260 : }
261 :
262 : /// @brief returns the euclidean distance in 3 dimensions
263 3457453963 : inline double distanceTo(const Position& p2) const {
264 3457453963 : return sqrt(distanceSquaredTo(p2));
265 : }
266 :
267 : /// @brief returns the square of the distance to another position
268 : inline double distanceSquaredTo(const Position& p2) const {
269 3457453963 : return (myX - p2.myX) * (myX - p2.myX) + (myY - p2.myY) * (myY - p2.myY) + (myZ - p2.myZ) * (myZ - p2.myZ);
270 : }
271 :
272 : /// @brief returns the euclidean distance in the x-y-plane
273 : inline double distanceTo2D(const Position& p2) const {
274 261638430 : return sqrt(distanceSquaredTo2D(p2));
275 : }
276 :
277 : /// @brief returns the square of the distance to another position (Only using x and y positions)
278 : inline double distanceSquaredTo2D(const Position& p2) const {
279 590241471 : return (myX - p2.myX) * (myX - p2.myX) + (myY - p2.myY) * (myY - p2.myY);
280 : }
281 :
282 : /// @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)
283 : inline double angleTo2D(const Position& other) const {
284 720853465 : return atan2(other.myY - myY, other.myX - myX);
285 : }
286 :
287 : /// @brief returns the slope of the vector pointing from here to the other position (in radians between -M_PI and M_PI)
288 157140076 : inline double slopeTo2D(const Position& other) const {
289 157140076 : return atan2(other.myZ - myZ, distanceTo2D(other));
290 : }
291 :
292 : /// @brief returns the cross product between this point and the second one
293 : Position crossProduct(const Position& pos) {
294 : return Position(
295 687 : myY * pos.myZ - myZ * pos.myY,
296 687 : myZ * pos.myX - myX * pos.myZ,
297 687 : myX * pos.myY - myY * pos.myX);
298 : }
299 :
300 : /// @brief returns the dot product (scalar product) between this point and the second one
301 : inline double dotProduct(const Position& pos) const {
302 13712 : return myX * pos.myX + myY * pos.myY + myZ * pos.myZ;
303 : }
304 :
305 : /// @brief rotate this position by rad around origin and return the result
306 : Position rotateAround2D(double rad, const Position& origin);
307 :
308 : /// @brief swap position X and Y
309 : void swapXY() {
310 : std::swap(myX, myY);
311 : }
312 :
313 : /// @brief check if position is NAN
314 : bool isNAN() const {
315 2856574 : return (std::isnan(myX) || std::isnan(myY) || std::isnan(myZ));
316 : }
317 :
318 : /// @brief round all coordinates to the given precision
319 : void round(int precision);
320 :
321 :
322 : /// @brief used to indicate that a position is valid
323 : static const Position INVALID;
324 :
325 : private:
326 : /// @brief The x-position
327 : double myX;
328 :
329 : /// @brief The y-position
330 : double myY;
331 :
332 : /// @brief The z-position
333 : double myZ;
334 : };
|
