Eclipse SUMO - Simulation of Urban MObility
Loading...
Searching...
No Matches
GeomHelper.cpp
Go to the documentation of this file.
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/****************************************************************************/
21// Some static methods performing geometrical operations
22/****************************************************************************/
23#include <config.h>
24
25#include <cmath>
26#include <limits>
27#include <algorithm>
28#include <iostream>
32#include "Boundary.h"
33#include "GeomHelper.h"
34
35// ===========================================================================
36// static members
37// ===========================================================================
38const double GeomHelper::INVALID_OFFSET = -1;
39
40
41// ===========================================================================
42// method definitions
43// ===========================================================================
44
45void
46GeomHelper::findLineCircleIntersections(const Position& c, double radius, const Position& p1, const Position& p2,
47 std::vector<double>& into) {
48 const double dx = p2.x() - p1.x();
49 const double dy = p2.y() - p1.y();
50
51 const double A = dx * dx + dy * dy;
52 const double B = 2 * (dx * (p1.x() - c.x()) + dy * (p1.y() - c.y()));
53 const double C = (p1.x() - c.x()) * (p1.x() - c.x()) + (p1.y() - c.y()) * (p1.y() - c.y()) - radius * radius;
54
55 const double det = B * B - 4 * A * C;
56 if ((A <= 0.0000001) || (det < 0)) {
57 // No real solutions.
58 return;
59 }
60 if (det == 0) {
61 // One solution.
62 const double t = -B / (2 * A);
63 if (t >= 0. && t <= 1.) {
64 into.push_back(t);
65 }
66 } else {
67 // Two solutions.
68 const double t = (double)((-B + sqrt(det)) / (2 * A));
69 Position intersection(p1.x() + t * dx, p1.y() + t * dy);
70 if (t >= 0. && t <= 1.) {
71 into.push_back(t);
72 }
73 const double t2 = (double)((-B - sqrt(det)) / (2 * A));
74 if (t2 >= 0. && t2 <= 1.) {
75 into.push_back(t2);
76 }
77 }
78}
79
80
81double
82GeomHelper::angle2D(const Position& p1, const Position& p2) {
83 return angleDiff(atan2(p1.y(), p1.x()), atan2(p2.y(), p2.x()));
84}
85
86
87double
89 const Position& lineEnd,
90 const Position& p, bool perpendicular) {
91 const double lineLength2D = lineStart.distanceTo2D(lineEnd);
92 if (lineLength2D == 0.) {
93 return 0.;
94 }
95 // scalar product equals length of orthogonal projection times length of vector being projected onto
96 // dividing the scalar product by the distance gives the relative position
97 const double u = (((p.x() - lineStart.x()) * (lineEnd.x() - lineStart.x())) +
98 ((p.y() - lineStart.y()) * (lineEnd.y() - lineStart.y()))
99 ) / lineLength2D;
100 if (u < 0. || u > lineLength2D) { // closest point does not fall within the line segment
101 if (perpendicular) {
102 return INVALID_OFFSET;
103 }
104 if (u < 0.) {
105 return 0.;
106 }
107 return lineLength2D;
108 }
109 return u;
110}
111
112
113double
115 const Position& lineEnd,
116 const Position& p, bool perpendicular) {
117 double result = nearest_offset_on_line_to_point2D(lineStart, lineEnd, p, perpendicular);
118 if (result != INVALID_OFFSET) {
119 const double lineLength2D = lineStart.distanceTo2D(lineEnd);
120 const double lineLength = lineStart.distanceTo(lineEnd);
121 result *= (lineLength / lineLength2D);
122 }
123 return result;
124}
125
128 if (v.intersects(Position(b.xmin(), b.ymin()), Position(b.xmin(), b.ymax()))) {
129 return v.intersectionPosition2D(
130 Position(b.xmin(), b.ymin()),
131 Position(b.xmin(), b.ymax()));
132 }
133 if (v.intersects(Position(b.xmax(), b.ymin()), Position(b.xmax(), b.ymax()))) {
134 return v.intersectionPosition2D(
135 Position(b.xmax(), b.ymin()),
136 Position(b.xmax(), b.ymax()));
137 }
138 if (v.intersects(Position(b.xmin(), b.ymin()), Position(b.xmax(), b.ymin()))) {
139 return v.intersectionPosition2D(
140 Position(b.xmin(), b.ymin()),
141 Position(b.xmax(), b.ymin()));
142 }
143 if (v.intersects(Position(b.xmin(), b.ymax()), Position(b.xmax(), b.ymax()))) {
144 return v.intersectionPosition2D(
145 Position(b.xmin(), b.ymax()),
146 Position(b.xmax(), b.ymax()));
147 }
148 throw 1;
149}
150
151double
152GeomHelper::getCCWAngleDiff(double angle1, double angle2) {
153 double v = angle2 - angle1;
154 if (v < 0) {
155 v = 360 + v;
156 }
157 return v;
158}
159
160
161double
162GeomHelper::getCWAngleDiff(double angle1, double angle2) {
163 double v = angle1 - angle2;
164 if (v < 0) {
165 v = 360 + v;
166 }
167 return v;
168}
169
170
171double
172GeomHelper::getMinAngleDiff(double angle1, double angle2) {
173 return MIN2(getCWAngleDiff(angle1, angle2), getCCWAngleDiff(angle1, angle2));
174}
175
176
177double
178GeomHelper::angleDiff(const double angle1, const double angle2) {
179 double dtheta = angle2 - angle1;
180 while (dtheta > (double) M_PI) {
181 dtheta -= (double)(2.0 * M_PI);
182 }
183 while (dtheta < (double) - M_PI) {
184 dtheta += (double)(2.0 * M_PI);
185 }
186 return dtheta;
187}
188
189
190double
191GeomHelper::naviDegree(const double angle) {
192 double degree = RAD2DEG(M_PI / 2. - angle);
193 if (std::isinf(degree)) {
194 //assert(false);
195 return 0;
196 }
197 while (degree >= 360.) {
198 degree -= 360.;
199 }
200 while (degree < 0.) {
201 degree += 360.;
202 }
203 return degree;
204}
205
206
207double
208GeomHelper::fromNaviDegree(const double angle) {
209 return M_PI / 2. - DEG2RAD(angle);
210}
211
212
213double
214GeomHelper::legacyDegree(const double angle, const bool positive) {
215 double degree = -RAD2DEG(M_PI / 2. + angle);
216 if (positive) {
217 while (degree >= 360.) {
218 degree -= 360.;
219 }
220 while (degree < 0.) {
221 degree += 360.;
222 }
223 } else {
224 while (degree >= 180.) {
225 degree -= 360.;
226 }
227 while (degree < -180.) {
228 degree += 360.;
229 }
230 }
231 return degree;
232}
233
235GeomHelper::makeCircle(const double radius, const Position& center, unsigned int nPoints) {
236 if (nPoints < 3) {
237 WRITE_ERROR(TL("GeomHelper::makeCircle() requires nPoints>=3"));
238 }
239 PositionVector circle;
240 circle.push_back({radius, 0});
241 for (unsigned int i = 1; i < nPoints; ++i) {
242 const double a = 2.0 * M_PI * (double)i / (double) nPoints;
243 circle.push_back({radius * cos(a), radius * sin(a)});
244 }
245 circle.push_back({radius, 0});
246 circle.add(center);
247 return circle;
248}
249
250
252GeomHelper::makeRing(const double radius1, const double radius2, const Position& center, unsigned int nPoints) {
253 if (nPoints < 3) {
254 WRITE_ERROR("GeomHelper::makeRing() requires nPoints>=3");
255 }
256 if (radius1 >= radius2) {
257 WRITE_ERROR("GeomHelper::makeRing() requires radius2>radius1");
258 }
259 PositionVector ring;
260 ring.push_back({radius1, 0});
261 ring.push_back({radius2, 0});
262 for (unsigned int i = 1; i < nPoints; ++i) {
263 const double a = 2.0 * M_PI * (double)i / (double) nPoints;
264 ring.push_back({radius2 * cos(a), radius2 * sin(a)});
265 }
266 ring.push_back({radius2, 0});
267 ring.push_back({radius1, 0});
268 for (unsigned int i = 1; i < nPoints; ++i) {
269 const double a = -2.0 * M_PI * (double)i / (double) nPoints;
270 ring.push_back({radius1 * cos(a), radius1 * sin(a)});
271 }
272 ring.push_back({radius1, 0});
273 ring.add(center);
274 return ring;
275}
276
277
278
279const Position
280GeomHelper::calculateLotSpacePosition(const PositionVector& shape, const int index, const double spaceDim, const double angle,
281 const double width, const double length) {
282 //declare pos
283 Position pos;
284 // declare shape offsets
285 const Position startOffset = shape.positionAtOffset(spaceDim * (index));
286 const Position endOffset = shape.positionAtOffset(spaceDim * (index + 1));
287 // continue depending of nagle
288 if (angle == 0) {
289 // parking parallel to the road
290 pos = endOffset;
291 } else {
292 // angled parking
293 const double hlp_angle = fabs(((double)atan2((endOffset.x() - startOffset.x()), (startOffset.y() - endOffset.y())) * (double)180.0 / (double)M_PI) - 180);
294 if (angle >= 0 && angle <= 90) {
295 pos.setx((startOffset.x() + endOffset.x()) / 2 - (width / 2) * (1 - cos(angle / 180 * M_PI)) * cos(hlp_angle / 180 * M_PI));
296 pos.sety((startOffset.y() + endOffset.y()) / 2 + (width / 2) * (1 - cos(angle / 180 * M_PI)) * sin(hlp_angle / 180 * M_PI));
297 pos.setz((startOffset.z() + endOffset.z()) / 2);
298 } else if (angle > 90 && angle <= 180) {
299 pos.setx((startOffset.x() + endOffset.x()) / 2 - (width / 2) * (1 + cos(angle / 180 * M_PI)) * cos(hlp_angle / 180 * M_PI));
300 pos.sety((startOffset.y() + endOffset.y()) / 2 + (width / 2) * (1 + cos(angle / 180 * M_PI)) * sin(hlp_angle / 180 * M_PI));
301 pos.setz((startOffset.z() + endOffset.z()) / 2);
302 } else if (angle > 180 && angle <= 270) {
303 pos.setx((startOffset.x() + endOffset.x()) / 2 - (length)*sin((angle - hlp_angle) / 180 * M_PI) - (width / 2) * (1 + cos(angle / 180 * M_PI)) * cos(hlp_angle / 180 * M_PI));
304 pos.sety((startOffset.y() + endOffset.y()) / 2 + (length)*cos((angle - hlp_angle) / 180 * M_PI) + (width / 2) * (1 + cos(angle / 180 * M_PI)) * sin(hlp_angle / 180 * M_PI));
305 pos.setz((startOffset.z() + endOffset.z()) / 2);
306 } else if (angle > 270 && angle < 360) {
307 pos.setx((startOffset.x() + endOffset.x()) / 2 - (length)*sin((angle - hlp_angle) / 180 * M_PI) - (width / 2) * (1 - cos(angle / 180 * M_PI)) * cos(hlp_angle / 180 * M_PI));
308 pos.sety((startOffset.y() + endOffset.y()) / 2 + (length)*cos((angle - hlp_angle) / 180 * M_PI) + (width / 2) * (1 - cos(angle / 180 * M_PI)) * sin(hlp_angle / 180 * M_PI));
309 pos.setz((startOffset.z() + endOffset.z()) / 2);
310 } else {
311 pos = (startOffset + endOffset) * 0.5;
312 }
313 }
314 return pos;
315}
316
317
318double
319GeomHelper::calculateLotSpaceAngle(const PositionVector& shape, const int index, const double spaceDim, const double angle) {
320 // declare shape offsets
321 const Position startOffset = shape.positionAtOffset(spaceDim * (index));
322 const Position endOffset = shape.positionAtOffset(spaceDim * (index + 1));
323 // return angle
324 return ((double)atan2((endOffset.x() - startOffset.x()), (startOffset.y() - endOffset.y())) * (double)180.0 / (double)M_PI) + angle;
325}
326
327
328double
329GeomHelper::calculateLotSpaceSlope(const PositionVector& shape, const int index, const double spaceDim) {
330 return shape.slopeDegreeAtOffset(spaceDim * (index + 1));
331}
332
333/****************************************************************************/
#define DEG2RAD(x)
Definition GeomHelper.h:35
#define M_PI
Definition GeomHelper.h:32
#define RAD2DEG(x)
Definition GeomHelper.h:36
#define WRITE_ERROR(msg)
Definition MsgHandler.h:304
#define TL(string)
Definition MsgHandler.h:315
T MIN2(T a, T b)
Definition StdDefs.h:76
A class that stores a 2D geometrical boundary.
Definition Boundary.h:39
double ymin() const
Returns minimum y-coordinate.
Definition Boundary.cpp:130
double xmin() const
Returns minimum x-coordinate.
Definition Boundary.cpp:118
double ymax() const
Returns maximum y-coordinate.
Definition Boundary.cpp:136
double xmax() const
Returns maximum x-coordinate.
Definition Boundary.cpp:124
static double getCCWAngleDiff(double angle1, double angle2)
Returns the distance of second angle from first angle counter-clockwise.
static void findLineCircleIntersections(const Position &c, double radius, const Position &p1, const Position &p2, std::vector< double > &into)
Returns the positions the given circle is crossed by the given line.
static double nearest_offset_on_line_to_point25D(const Position &lineStart, const Position &lineEnd, const Position &p, bool perpendicular=true)
static const Position calculateLotSpacePosition(const PositionVector &shape, const int index, const double spaceDim, const double angle, const double width, const double length)
calculate lotSpace position
static double calculateLotSpaceAngle(const PositionVector &shape, const int index, const double spaceDim, const double angle)
calculate lotSpace angle
static Position crossPoint(const Boundary &b, const PositionVector &v)
static double angle2D(const Position &p1, const Position &p2)
Returns the angle between two vectors on a plane The angle is from vector 1 to vector 2,...
static PositionVector makeRing(const double radius1, const double radius2, const Position &center, unsigned int nPoints)
static const double INVALID_OFFSET
a value to signify offsets outside the range of [0, Line.length()]
Definition GeomHelper.h:50
static double getCWAngleDiff(double angle1, double angle2)
Returns the distance of second angle from first angle clockwise.
static double calculateLotSpaceSlope(const PositionVector &shape, const int index, const double spaceDim)
calculate lotSpace slope
static double nearest_offset_on_line_to_point2D(const Position &lineStart, const Position &lineEnd, const Position &p, bool perpendicular=true)
static PositionVector makeCircle(const double radius, const Position &center, unsigned int nPoints)
static double naviDegree(const double angle)
static double fromNaviDegree(const double angle)
static double legacyDegree(const double angle, const bool positive=false)
static double angleDiff(const double angle1, const double angle2)
Returns the difference of the second angle to the first angle in radiants.
static double getMinAngleDiff(double angle1, double angle2)
Returns the minimum distance (clockwise/counter-clockwise) between both angles.
A point in 2D or 3D with translation and scaling methods.
Definition Position.h:37
void setx(double x)
set position x
Definition Position.h:70
double distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
Definition Position.h:276
double distanceTo(const Position &p2) const
returns the euclidean distance in 3 dimensions
Definition Position.h:266
double x() const
Returns the x-position.
Definition Position.h:55
void setz(double z)
set position z
Definition Position.h:80
double z() const
Returns the z-position.
Definition Position.h:65
void sety(double y)
set position y
Definition Position.h:75
double y() const
Returns the y-position.
Definition Position.h:60
A list of positions.
Position intersectionPosition2D(const Position &p1, const Position &p2, const double withinDist=0.) const
Returns the position of the intersection.
Position positionAtOffset(double pos, double lateralOffset=0) const
Returns the position at the given length.
void add(double xoff, double yoff, double zoff)
double slopeDegreeAtOffset(double pos) const
Returns the slope at the given length.
bool intersects(const Position &p1, const Position &p2) const
Returns the information whether this list of points interesects the given line.