Eclipse SUMO - Simulation of Urban MObility
GeomHelper.cpp
Go to the documentation of this file.
1/****************************************************************************/
2// Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3// Copyright (C) 2001-2022 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.0f) {
93 return 0.0f;
94 } else {
95 // scalar product equals length of orthogonal projection times length of vector being projected onto
96 // dividing the scalar product by the square of 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 * lineLength2D);
100 if (u < 0.0f || u > 1.0f) { // closest point does not fall within the line segment
101 if (perpendicular) {
102 return INVALID_OFFSET;
103 }
104 if (u < 0.0f) {
105 return 0.0f;
106 }
107 return lineLength2D;
108 }
109 return u * lineLength2D;
110 }
111}
112
113
114double
116 const Position& lineEnd,
117 const Position& p, bool perpendicular) {
118 double result = nearest_offset_on_line_to_point2D(lineStart, lineEnd, p, perpendicular);
119 if (result != INVALID_OFFSET) {
120 const double lineLength2D = lineStart.distanceTo2D(lineEnd);
121 const double lineLength = lineStart.distanceTo(lineEnd);
122 result *= (lineLength / lineLength2D);
123 }
124 return result;
125}
126
129 if (v.intersects(Position(b.xmin(), b.ymin()), Position(b.xmin(), b.ymax()))) {
130 return v.intersectionPosition2D(
131 Position(b.xmin(), b.ymin()),
132 Position(b.xmin(), b.ymax()));
133 }
134 if (v.intersects(Position(b.xmax(), b.ymin()), Position(b.xmax(), b.ymax()))) {
135 return v.intersectionPosition2D(
136 Position(b.xmax(), b.ymin()),
137 Position(b.xmax(), b.ymax()));
138 }
139 if (v.intersects(Position(b.xmin(), b.ymin()), Position(b.xmax(), b.ymin()))) {
140 return v.intersectionPosition2D(
141 Position(b.xmin(), b.ymin()),
142 Position(b.xmax(), b.ymin()));
143 }
144 if (v.intersects(Position(b.xmin(), b.ymax()), Position(b.xmax(), b.ymax()))) {
145 return v.intersectionPosition2D(
146 Position(b.xmin(), b.ymax()),
147 Position(b.xmax(), b.ymax()));
148 }
149 throw 1;
150}
151
152double
153GeomHelper::getCCWAngleDiff(double angle1, double angle2) {
154 double v = angle2 - angle1;
155 if (v < 0) {
156 v = 360 + v;
157 }
158 return v;
159}
160
161
162double
163GeomHelper::getCWAngleDiff(double angle1, double angle2) {
164 double v = angle1 - angle2;
165 if (v < 0) {
166 v = 360 + v;
167 }
168 return v;
169}
170
171
172double
173GeomHelper::getMinAngleDiff(double angle1, double angle2) {
174 return MIN2(getCWAngleDiff(angle1, angle2), getCCWAngleDiff(angle1, angle2));
175}
176
177
178double
179GeomHelper::angleDiff(const double angle1, const double angle2) {
180 double dtheta = angle2 - angle1;
181 while (dtheta > (double) M_PI) {
182 dtheta -= (double)(2.0 * M_PI);
183 }
184 while (dtheta < (double) - M_PI) {
185 dtheta += (double)(2.0 * M_PI);
186 }
187 return dtheta;
188}
189
190
191double
192GeomHelper::naviDegree(const double angle) {
193 double degree = RAD2DEG(M_PI / 2. - angle);
194 if (std::isinf(degree)) {
195 //assert(false);
196 return 0;
197 }
198 while (degree >= 360.) {
199 degree -= 360.;
200 }
201 while (degree < 0.) {
202 degree += 360.;
203 }
204 return degree;
205}
206
207
208double
209GeomHelper::fromNaviDegree(const double angle) {
210 return M_PI / 2. - DEG2RAD(angle);
211}
212
213
214double
215GeomHelper::legacyDegree(const double angle, const bool positive) {
216 double degree = -RAD2DEG(M_PI / 2. + angle);
217 if (positive) {
218 while (degree >= 360.) {
219 degree -= 360.;
220 }
221 while (degree < 0.) {
222 degree += 360.;
223 }
224 } else {
225 while (degree >= 180.) {
226 degree -= 360.;
227 }
228 while (degree < -180.) {
229 degree += 360.;
230 }
231 }
232 return degree;
233}
234
236GeomHelper::makeCircle(const double radius, const Position& center, unsigned int nPoints) {
237 if (nPoints < 3) {
238 WRITE_ERROR(TL("GeomHelper::makeCircle() requires nPoints>=3"));
239 }
240 PositionVector circle;
241 circle.push_back({radius, 0});
242 for (unsigned int i = 1; i < nPoints; ++i) {
243 const double a = 2.0 * M_PI * (double)i / (double) nPoints;
244 circle.push_back({radius * cos(a), radius * sin(a)});
245 }
246 circle.push_back({radius, 0});
247 circle.add(center);
248 return circle;
249}
250
251
253GeomHelper::makeRing(const double radius1, const double radius2, const Position& center, unsigned int nPoints) {
254 if (nPoints < 3) {
255 WRITE_ERROR(TL("GeomHelper::makeRing() requires nPoints>=3"));
256 }
257 if (radius1 >= radius2) {
258 WRITE_ERROR(TL("GeomHelper::makeRing() requires radius2>radius1"));
259 }
260 PositionVector ring;
261 ring.push_back({radius1, 0});
262 ring.push_back({radius2, 0});
263 for (unsigned int i = 1; i < nPoints; ++i) {
264 const double a = 2.0 * M_PI * (double)i / (double) nPoints;
265 ring.push_back({radius2 * cos(a), radius2 * sin(a)});
266 }
267 ring.push_back({radius2, 0});
268 ring.push_back({radius1, 0});
269 for (unsigned int i = 1; i < nPoints; ++i) {
270 const double a = -2.0 * M_PI * (double)i / (double) nPoints;
271 ring.push_back({radius1 * cos(a), radius1 * sin(a)});
272 }
273 ring.push_back({radius1, 0});
274 ring.add(center);
275 return ring;
276}
277
278
279
280const Position
281GeomHelper::calculateLotSpacePosition(const PositionVector& shape, const int index, const double spaceDim, const double angle,
282 const double width, const double length) {
283 //declare pos
284 Position pos;
285 // declare shape offsets
286 const Position startOffset = shape.positionAtOffset(spaceDim * (index));
287 const Position endOffset = shape.positionAtOffset(spaceDim * (index + 1));
288 // continue depending of nagle
289 if (angle == 0) {
290 // parking parallel to the road
291 pos = endOffset;
292 } else {
293 // angled parking
294 const double hlp_angle = fabs(((double)atan2((endOffset.x() - startOffset.x()), (startOffset.y() - endOffset.y())) * (double)180.0 / (double)M_PI) - 180);
295 if (angle >= 0 && angle <= 90) {
296 pos.setx((startOffset.x() + endOffset.x()) / 2 - (width / 2) * (1 - cos(angle / 180 * M_PI)) * cos(hlp_angle / 180 * M_PI));
297 pos.sety((startOffset.y() + endOffset.y()) / 2 + (width / 2) * (1 - cos(angle / 180 * M_PI)) * sin(hlp_angle / 180 * M_PI));
298 pos.setz((startOffset.z() + endOffset.z()) / 2);
299 } else if (angle > 90 && angle <= 180) {
300 pos.setx((startOffset.x() + endOffset.x()) / 2 - (width / 2) * (1 + cos(angle / 180 * M_PI)) * cos(hlp_angle / 180 * M_PI));
301 pos.sety((startOffset.y() + endOffset.y()) / 2 + (width / 2) * (1 + cos(angle / 180 * M_PI)) * sin(hlp_angle / 180 * M_PI));
302 pos.setz((startOffset.z() + endOffset.z()) / 2);
303 } else if (angle > 180 && angle <= 270) {
304 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));
305 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));
306 pos.setz((startOffset.z() + endOffset.z()) / 2);
307 } else if (angle > 270 && angle < 360) {
308 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));
309 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));
310 pos.setz((startOffset.z() + endOffset.z()) / 2);
311 } else {
312 pos = (startOffset + endOffset) * 0.5;
313 }
314 }
315 return pos;
316}
317
318
319double
320GeomHelper::calculateLotSpaceAngle(const PositionVector& shape, const int index, const double spaceDim, const double angle) {
321 // declare shape offsets
322 const Position startOffset = shape.positionAtOffset(spaceDim * (index));
323 const Position endOffset = shape.positionAtOffset(spaceDim * (index + 1));
324 // return angle
325 return ((double)atan2((endOffset.x() - startOffset.x()), (startOffset.y() - endOffset.y())) * (double)180.0 / (double)M_PI) + angle;
326}
327
328
329double
330GeomHelper::calculateLotSpaceSlope(const PositionVector& shape, const int index, const double spaceDim) {
331 return shape.slopeDegreeAtOffset(spaceDim * (index + 1));
332}
333
334/****************************************************************************/
#define DEG2RAD(x)
Definition: GeomHelper.h:35
#define RAD2DEG(x)
Definition: GeomHelper.h:36
#define WRITE_ERROR(msg)
Definition: MsgHandler.h:274
#define TL(string)
Definition: MsgHandler.h:282
T MIN2(T a, T b)
Definition: StdDefs.h:71
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.
Definition: GeomHelper.cpp:153
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.
Definition: GeomHelper.cpp:46
static double nearest_offset_on_line_to_point25D(const Position &lineStart, const Position &lineEnd, const Position &p, bool perpendicular=true)
Definition: GeomHelper.cpp:115
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
Definition: GeomHelper.cpp:281
static double calculateLotSpaceAngle(const PositionVector &shape, const int index, const double spaceDim, const double angle)
calculate lotSpace angle
Definition: GeomHelper.cpp:320
static Position crossPoint(const Boundary &b, const PositionVector &v)
Definition: GeomHelper.cpp:128
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,...
Definition: GeomHelper.cpp:82
static PositionVector makeRing(const double radius1, const double radius2, const Position &center, unsigned int nPoints)
Definition: GeomHelper.cpp:253
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.
Definition: GeomHelper.cpp:163
static double calculateLotSpaceSlope(const PositionVector &shape, const int index, const double spaceDim)
calculate lotSpace slope
Definition: GeomHelper.cpp:330
static double nearest_offset_on_line_to_point2D(const Position &lineStart, const Position &lineEnd, const Position &p, bool perpendicular=true)
Definition: GeomHelper.cpp:88
static PositionVector makeCircle(const double radius, const Position &center, unsigned int nPoints)
Definition: GeomHelper.cpp:236
static double naviDegree(const double angle)
Definition: GeomHelper.cpp:192
static double fromNaviDegree(const double angle)
Definition: GeomHelper.cpp:209
static double legacyDegree(const double angle, const bool positive=false)
Definition: GeomHelper.cpp:215
static double angleDiff(const double angle1, const double angle2)
Returns the difference of the second angle to the first angle in radiants.
Definition: GeomHelper.cpp:179
static double getMinAngleDiff(double angle1, double angle2)
Returns the minimum distance (clockwise/counter-clockwise) between both angles.
Definition: GeomHelper.cpp:173
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:252
double distanceTo(const Position &p2) const
returns the euclidean distance in 3 dimension
Definition: Position.h:242
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.
#define M_PI
Definition: odrSpiral.cpp:45