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 963297 : Position() :
41 786939463 : myX(0.0), myY(0.0), myZ(0.0) { }
42 :
43 : /// @brief Parametrised constructor (only for x-y)
44 187935536 : Position(double x, double y) :
45 33698386 : myX(x), myY(y), myZ(0) { }
46 :
47 : /// @brief Parametrised constructor
48 2192245 : Position(double x, double y, double z) :
49 21368509 : myX(x), myY(y), myZ(z) { }
50 :
51 : /// @brief Destructor
52 3980799790 : ~Position() { }
53 :
54 : /// @brief Returns the x-position
55 : inline double x() const {
56 420961855 : return myX;
57 : }
58 :
59 : /// @brief Returns the y-position
60 : inline double y() const {
61 617685291 : return myY;
62 : }
63 :
64 : /// @brief Returns the z-position
65 : inline double z() const {
66 31594798 : return myZ;
67 : }
68 :
69 : /// @brief set position x
70 : void setx(double x) {
71 1094 : myX = x;
72 2 : }
73 :
74 : /// @brief set position y
75 : void sety(double y) {
76 6460 : myY = y;
77 26 : }
78 :
79 : /// @brief set position z
80 : void setz(double z) {
81 702607 : myZ = z;
82 5779 : }
83 :
84 : /// @brief set positions x and y
85 : void set(double x, double y) {
86 730092 : myX = x;
87 145654 : myY = y;
88 : }
89 :
90 : /// @brief set positions x, y and z
91 : void set(double x, double y, double z) {
92 544328 : myX = x;
93 544328 : myY = y;
94 2863733 : myZ = z;
95 10112 : }
96 :
97 : /// @brief set position with another position
98 : void set(const Position& pos) {
99 562833 : myX = pos.myX;
100 562833 : myY = pos.myY;
101 562833 : myZ = pos.myZ;
102 5 : }
103 :
104 : /// @brief Multiplies position with the given value
105 : void mul(double val) {
106 37848 : myX *= val;
107 37848 : myY *= val;
108 37807 : myZ *= val;
109 25862 : }
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 8735782 : myX += pos.myX;
134 8735782 : myY += pos.myY;
135 8735782 : myZ += pos.myZ;
136 7981658 : }
137 :
138 : /// @brief Adds the given position to this one
139 : void add(double dx, double dy) {
140 2114 : myX += dx;
141 2114 : myY += dy;
142 868 : }
143 :
144 : /// @brief Adds the given position to this one
145 : void add(double dx, double dy, double dz) {
146 5077899 : myX += dx;
147 5077899 : myY += dy;
148 5077899 : myZ += dz;
149 : }
150 :
151 : /// @brief Subtracts the given position from this one
152 : void sub(double dx, double dy) {
153 22277 : myX -= dx;
154 22277 : 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 10166509 : myX -= pos.myX;
167 10166509 : myY -= pos.myY;
168 10166509 : myZ -= pos.myZ;
169 4936068 : }
170 :
171 : /// @brief Computes the length of the given vector
172 : inline double length() const {
173 12830 : return sqrt(myX * myX + myY * myY + myZ * myZ);
174 : }
175 :
176 : /// @brief Computes the length of the given vector neglecting the z coordinate
177 : inline double length2D() const {
178 2816 : return sqrt(myX * myX + myY * myY);
179 : }
180 :
181 : /// @brief Normalizes the given vector
182 1592 : inline void norm2D() {
183 : const double val = length2D();
184 1592 : if (val != 0.) {
185 1592 : myX /= val;
186 1592 : myY /= val;
187 : }
188 1592 : }
189 :
190 : /// @brief output operator
191 1686984 : friend std::ostream& operator<<(std::ostream& os, const Position& p) {
192 : os << p.x() << "," << p.y();
193 1686984 : if (p.z() != double(0.0)) {
194 : os << "," << p.z();
195 : }
196 1686984 : return os;
197 : }
198 :
199 : /// @brief add operator
200 : Position operator+(const Position& p2) const {
201 22386488 : return Position(myX + p2.myX, myY + p2.myY, myZ + p2.myZ);
202 : }
203 :
204 : /// @brief sub operator
205 : Position operator-(const Position& p2) const {
206 47641811 : return Position(myX - p2.myX, myY - p2.myY, myZ - p2.myZ);
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 1492553398 : 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 : Position operator/(double scalar) const {
216 3 : return Position(myX / scalar, myY / scalar, myZ / scalar);
217 : }
218 :
219 : /// @brief keep the direction but modify the length of the (location) vector to length + scalar
220 5249 : Position operator+(double offset) const {
221 5249 : const double length = distanceTo(Position(0, 0, 0));
222 5249 : if (length == 0) {
223 0 : return *this;
224 : }
225 5249 : const double scalar = (length + offset) / length;
226 5249 : return Position(myX * scalar, myY * scalar, myZ * scalar);
227 : }
228 :
229 : /// @brief keep the direction but modify the length of the (location) vector to length - scalar
230 : Position operator-(double offset) const {
231 : const double length = distanceTo(Position(0, 0, 0));
232 : if (length == 0) {
233 : return *this;
234 : }
235 : const double scalar = (length - offset) / length;
236 : return Position(myX * scalar, myY * scalar, myZ * scalar);
237 : }
238 :
239 : /// @brief comparation operator
240 : bool operator==(const Position& p2) const {
241 2245211314 : return myX == p2.myX && myY == p2.myY && myZ == p2.myZ;
242 : }
243 :
244 : /// @brief difference operator
245 : bool operator!=(const Position& p2) const {
246 878984105 : return myX != p2.myX || myY != p2.myY || myZ != p2.myZ;
247 : }
248 :
249 : /// @brief lexicographical sorting for use in maps and sets
250 : bool operator<(const Position& p2) const {
251 30505 : if (myX != p2.myX) {
252 22650 : return myX < p2.myX;
253 : }
254 7855 : if (myY != p2.myY) {
255 25 : return myY < p2.myY;
256 : }
257 7830 : return myZ < p2.myZ;
258 : }
259 :
260 : /// @brief check whether the other position has a euclidean distance of less than maxDiv
261 : bool almostSame(const Position& p2, double maxDiv = POSITION_EPS) const {
262 7823295 : return distanceTo(p2) < maxDiv;
263 : }
264 :
265 : /// @brief returns the euclidean distance in 3 dimensions
266 3210214659 : inline double distanceTo(const Position& p2) const {
267 3210214659 : return sqrt(distanceSquaredTo(p2));
268 : }
269 :
270 : /// @brief returns the square of the distance to another position
271 : inline double distanceSquaredTo(const Position& p2) const {
272 3210214659 : return (myX - p2.myX) * (myX - p2.myX) + (myY - p2.myY) * (myY - p2.myY) + (myZ - p2.myZ) * (myZ - p2.myZ);
273 : }
274 :
275 : /// @brief returns the euclidean distance in the x-y-plane
276 : inline double distanceTo2D(const Position& p2) const {
277 254103765 : return sqrt(distanceSquaredTo2D(p2));
278 : }
279 :
280 : /// @brief returns the square of the distance to another position (Only using x and y positions)
281 : inline double distanceSquaredTo2D(const Position& p2) const {
282 591764170 : return (myX - p2.myX) * (myX - p2.myX) + (myY - p2.myY) * (myY - p2.myY);
283 : }
284 :
285 : /// @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)
286 : inline double angleTo2D(const Position& other) const {
287 666446776 : return atan2(other.myY - myY, other.myX - myX);
288 : }
289 :
290 : /// @brief returns the slope of the vector pointing from here to the other position (in radians between -M_PI and M_PI)
291 151734702 : inline double slopeTo2D(const Position& other) const {
292 151734702 : return atan2(other.myZ - myZ, distanceTo2D(other));
293 : }
294 :
295 : /// @brief returns the cross product between this point and the second one
296 : Position crossProduct(const Position& pos) {
297 : return Position(
298 357 : myY * pos.myZ - myZ * pos.myY,
299 357 : myZ * pos.myX - myX * pos.myZ,
300 357 : myX * pos.myY - myY * pos.myX);
301 : }
302 :
303 : /// @brief returns the dot product (scalar product) between this point and the second one
304 : inline double dotProduct(const Position& pos) const {
305 13196 : return myX * pos.myX + myY * pos.myY + myZ * pos.myZ;
306 : }
307 :
308 : /// @brief rotate this position by rad around origin and return the result
309 : Position rotateAround2D(double rad, const Position& origin);
310 :
311 : /// @brief swap position X and Y
312 : void swapXY() {
313 : std::swap(myX, myY);
314 : }
315 :
316 : /// @brief check if position is NAN
317 : bool isNAN() const {
318 9353 : return (std::isnan(myX) || std::isnan(myY) || std::isnan(myZ));
319 : }
320 :
321 : /// @brief used to indicate that a position is valid
322 : static const Position INVALID;
323 :
324 : private:
325 : /// @brief The x-position
326 : double myX;
327 :
328 : /// @brief The y-position
329 : double myY;
330 :
331 : /// @brief The z-position
332 : double myZ;
333 : };
|
