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 GeomHelper.cpp
15 : /// @author Daniel Krajzewicz
16 : /// @author Friedemann Wesner
17 : /// @author Jakob Erdmann
18 : /// @author Michael Behrisch
19 : /// @author Mirko Barthauer
20 : /// @date Sept 2002
21 : ///
22 : // Some static methods performing geometrical operations
23 : /****************************************************************************/
24 : #include <config.h>
25 :
26 : #include <cmath>
27 : #include <limits>
28 : #include <algorithm>
29 : #include <iostream>
30 : #include <utils/common/StdDefs.h>
31 : #include <utils/common/ToString.h>
32 : #include <utils/common/MsgHandler.h>
33 : #include "Boundary.h"
34 : #include "GeomHelper.h"
35 :
36 : // ===========================================================================
37 : // static members
38 : // ===========================================================================
39 : const double GeomHelper::INVALID_OFFSET = -1;
40 :
41 :
42 : // ===========================================================================
43 : // method definitions
44 : // ===========================================================================
45 :
46 : void
47 58616 : GeomHelper::findLineCircleIntersections(const Position& c, double radius, const Position& p1, const Position& p2,
48 : std::vector<double>& into) {
49 58616 : const double dx = p2.x() - p1.x();
50 58616 : const double dy = p2.y() - p1.y();
51 :
52 58616 : const double A = dx * dx + dy * dy;
53 58616 : const double B = 2 * (dx * (p1.x() - c.x()) + dy * (p1.y() - c.y()));
54 58616 : const double C = (p1.x() - c.x()) * (p1.x() - c.x()) + (p1.y() - c.y()) * (p1.y() - c.y()) - radius * radius;
55 :
56 58616 : const double det = B * B - 4 * A * C;
57 58616 : if ((A <= 0.0000001) || (det < 0)) {
58 : // No real solutions.
59 : return;
60 : }
61 39164 : if (det == 0) {
62 : // One solution.
63 0 : const double t = -B / (2 * A);
64 0 : if (t >= 0. && t <= 1.) {
65 0 : into.push_back(t);
66 : }
67 : } else {
68 : // Two solutions.
69 39164 : const double t = (double)((-B + sqrt(det)) / (2 * A));
70 : Position intersection(p1.x() + t * dx, p1.y() + t * dy);
71 39164 : if (t >= 0. && t <= 1.) {
72 492 : into.push_back(t);
73 : }
74 39164 : const double t2 = (double)((-B - sqrt(det)) / (2 * A));
75 39164 : if (t2 >= 0. && t2 <= 1.) {
76 520 : into.push_back(t2);
77 : }
78 : }
79 : }
80 :
81 :
82 : double
83 182330593 : GeomHelper::angle2D(const Position& p1, const Position& p2) {
84 182330593 : return angleDiff(atan2(p1.y(), p1.x()), atan2(p2.y(), p2.x()));
85 : }
86 :
87 :
88 : double
89 79447896 : GeomHelper::nearest_offset_on_line_to_point2D(const Position& lineStart,
90 : const Position& lineEnd,
91 : const Position& p, bool perpendicular) {
92 : const double lineLength2D = lineStart.distanceTo2D(lineEnd);
93 79447896 : if (lineLength2D == 0.) {
94 : return 0.;
95 : }
96 : // scalar product equals length of orthogonal projection times length of vector being projected onto
97 : // dividing the scalar product by the distance gives the relative position
98 79291894 : const double u = (((p.x() - lineStart.x()) * (lineEnd.x() - lineStart.x())) +
99 79291894 : ((p.y() - lineStart.y()) * (lineEnd.y() - lineStart.y()))
100 79291894 : ) / lineLength2D;
101 79291894 : if (u < 0. || u > lineLength2D) { // closest point does not fall within the line segment
102 57951069 : if (perpendicular) {
103 : return INVALID_OFFSET;
104 : }
105 50472131 : if (u < 0.) {
106 : return 0.;
107 : }
108 21175063 : return lineLength2D;
109 : }
110 : return u;
111 : }
112 :
113 :
114 : double
115 0 : GeomHelper::nearest_offset_on_line_to_point25D(const Position& lineStart,
116 : const Position& lineEnd,
117 : const Position& p, bool perpendicular) {
118 0 : double result = nearest_offset_on_line_to_point2D(lineStart, lineEnd, p, perpendicular);
119 0 : if (result != INVALID_OFFSET) {
120 : const double lineLength2D = lineStart.distanceTo2D(lineEnd);
121 0 : const double lineLength = lineStart.distanceTo(lineEnd);
122 0 : result *= (lineLength / lineLength2D);
123 : }
124 0 : return result;
125 : }
126 :
127 : Position
128 0 : GeomHelper::crossPoint(const Boundary& b, const PositionVector& v) {
129 0 : if (v.intersects(Position(b.xmin(), b.ymin()), Position(b.xmin(), b.ymax()))) {
130 0 : return v.intersectionPosition2D(
131 0 : Position(b.xmin(), b.ymin()),
132 0 : Position(b.xmin(), b.ymax()));
133 : }
134 0 : if (v.intersects(Position(b.xmax(), b.ymin()), Position(b.xmax(), b.ymax()))) {
135 0 : return v.intersectionPosition2D(
136 0 : Position(b.xmax(), b.ymin()),
137 0 : Position(b.xmax(), b.ymax()));
138 : }
139 0 : if (v.intersects(Position(b.xmin(), b.ymin()), Position(b.xmax(), b.ymin()))) {
140 0 : return v.intersectionPosition2D(
141 0 : Position(b.xmin(), b.ymin()),
142 0 : Position(b.xmax(), b.ymin()));
143 : }
144 0 : if (v.intersects(Position(b.xmin(), b.ymax()), Position(b.xmax(), b.ymax()))) {
145 0 : return v.intersectionPosition2D(
146 0 : Position(b.xmin(), b.ymax()),
147 0 : Position(b.xmax(), b.ymax()));
148 : }
149 0 : throw 1;
150 : }
151 :
152 : double
153 1355491 : GeomHelper::getCCWAngleDiff(double angle1, double angle2) {
154 1355491 : double v = angle2 - angle1;
155 1355491 : if (v < 0) {
156 612279 : v = 360 + v;
157 : }
158 1355491 : return v;
159 : }
160 :
161 :
162 : double
163 1365093 : GeomHelper::getCWAngleDiff(double angle1, double angle2) {
164 1365093 : double v = angle1 - angle2;
165 1365093 : if (v < 0) {
166 695065 : v = 360 + v;
167 : }
168 1365093 : return v;
169 : }
170 :
171 :
172 : double
173 1350703 : GeomHelper::getMinAngleDiff(double angle1, double angle2) {
174 1350703 : return MIN2(getCWAngleDiff(angle1, angle2), getCCWAngleDiff(angle1, angle2));
175 : }
176 :
177 :
178 : double
179 189792268 : GeomHelper::angleDiff(const double angle1, const double angle2) {
180 189792268 : double dtheta = angle2 - angle1;
181 192425066 : while (dtheta > (double) M_PI) {
182 2632798 : dtheta -= (double)(2.0 * M_PI);
183 : }
184 192196275 : while (dtheta < (double) - M_PI) {
185 2404007 : dtheta += (double)(2.0 * M_PI);
186 : }
187 189792268 : return dtheta;
188 : }
189 :
190 :
191 : double
192 3963651 : GeomHelper::naviDegree(const double angle) {
193 3963651 : double degree = RAD2DEG(M_PI / 2. - angle);
194 3963651 : if (std::isinf(degree)) {
195 : //assert(false);
196 : return 0;
197 : }
198 3963731 : while (degree >= 360.) {
199 80 : degree -= 360.;
200 : }
201 4752224 : while (degree < 0.) {
202 788573 : degree += 360.;
203 : }
204 : return degree;
205 : }
206 :
207 :
208 : double
209 25947 : GeomHelper::fromNaviDegree(const double angle) {
210 25947 : return M_PI / 2. - DEG2RAD(angle);
211 : }
212 :
213 :
214 : double
215 23911249 : GeomHelper::legacyDegree(const double angle, const bool positive) {
216 23911249 : double degree = -RAD2DEG(M_PI / 2. + angle);
217 23911249 : if (positive) {
218 2538735 : while (degree >= 360.) {
219 0 : degree -= 360.;
220 : }
221 4462835 : while (degree < 0.) {
222 1924100 : degree += 360.;
223 : }
224 : } else {
225 21372514 : while (degree >= 180.) {
226 0 : degree -= 360.;
227 : }
228 26695663 : while (degree < -180.) {
229 5323149 : degree += 360.;
230 : }
231 : }
232 23911249 : return degree;
233 : }
234 :
235 : PositionVector
236 0 : GeomHelper::makeCircle(const double radius, const Position& center, unsigned int nPoints) {
237 0 : if (nPoints < 3) {
238 0 : WRITE_ERROR(TL("GeomHelper::makeCircle() requires nPoints>=3"));
239 : }
240 0 : PositionVector circle;
241 0 : circle.push_back({radius, 0});
242 0 : for (unsigned int i = 1; i < nPoints; ++i) {
243 0 : const double a = 2.0 * M_PI * (double)i / (double) nPoints;
244 0 : circle.push_back({radius * cos(a), radius * sin(a)});
245 : }
246 0 : circle.push_back({radius, 0});
247 0 : circle.add(center);
248 0 : return circle;
249 0 : }
250 :
251 :
252 : PositionVector
253 69 : GeomHelper::makeRing(const double radius1, const double radius2, const Position& center, unsigned int nPoints) {
254 69 : if (nPoints < 3) {
255 0 : WRITE_ERROR("GeomHelper::makeRing() requires nPoints>=3");
256 : }
257 69 : if (radius1 >= radius2) {
258 0 : WRITE_ERROR("GeomHelper::makeRing() requires radius2>radius1");
259 : }
260 69 : PositionVector ring;
261 69 : ring.push_back({radius1, 0});
262 69 : ring.push_back({radius2, 0});
263 2346 : for (unsigned int i = 1; i < nPoints; ++i) {
264 2277 : const double a = 2.0 * M_PI * (double)i / (double) nPoints;
265 2277 : ring.push_back({radius2 * cos(a), radius2 * sin(a)});
266 : }
267 69 : ring.push_back({radius2, 0});
268 69 : ring.push_back({radius1, 0});
269 2346 : for (unsigned int i = 1; i < nPoints; ++i) {
270 2277 : const double a = -2.0 * M_PI * (double)i / (double) nPoints;
271 2277 : ring.push_back({radius1 * cos(a), radius1 * sin(a)});
272 : }
273 69 : ring.push_back({radius1, 0});
274 69 : ring.add(center);
275 69 : return ring;
276 0 : }
277 :
278 :
279 :
280 : const Position
281 50160 : GeomHelper::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 50160 : const Position startOffset = shape.positionAtOffset(spaceDim * (index));
287 50160 : const Position endOffset = shape.positionAtOffset(spaceDim * (index + 1));
288 : // continue depending of nagle
289 50160 : if (angle == 0) {
290 : // parking parallel to the road
291 48441 : pos = endOffset;
292 : } else {
293 : // angled parking
294 : double normAngle = angle;
295 1822 : while (normAngle < 0) {
296 103 : normAngle += 360.;
297 : }
298 1719 : normAngle = fmod(normAngle, 360);
299 1719 : const double radianAngle = normAngle / 180 * M_PI;
300 1719 : double spaceExtension = width * sin(radianAngle) + length * cos(radianAngle);
301 1719 : const double hlp_angle = fabs(((double)atan2((endOffset.y() - startOffset.y()), (endOffset.x() - startOffset.x()))));
302 : Position offset;
303 1719 : double xOffset = 0.5 * width * sin(radianAngle) - 0.5 * (spaceExtension - spaceDim);
304 1719 : pos.setx(startOffset.x() + xOffset + length * cos(radianAngle));
305 1719 : if (normAngle <= 90) {
306 1290 : pos.sety((startOffset.y() + 0.5 * width * (1 - cos(radianAngle)) - length * sin(radianAngle)));
307 429 : } else if (normAngle <= 180) {
308 6 : pos.sety(startOffset.y() + 0.5 * width * (1 + cos(radianAngle)) - length * sin(radianAngle));
309 423 : } else if (angle <= 270) {
310 423 : pos.sety(startOffset.y() + 0.5 * width * (1 - cos(radianAngle - M_PI)));
311 : } else {
312 0 : pos.sety(startOffset.y() + 0.5 * width * (1 + cos(radianAngle - M_PI)));
313 : }
314 1719 : pos.setz((startOffset.z() + endOffset.z()) / 2);
315 1719 : pos = pos.rotateAround2D(hlp_angle, startOffset);
316 : }
317 50160 : return pos;
318 : }
319 :
320 :
321 : double
322 50160 : GeomHelper::calculateLotSpaceAngle(const PositionVector& shape, const int index, const double spaceDim, const double angle) {
323 : // declare shape offsets
324 50160 : const Position startOffset = shape.positionAtOffset(spaceDim * (index));
325 50160 : const Position endOffset = shape.positionAtOffset(spaceDim * (index + 1));
326 : // return angle
327 50160 : return ((double)atan2((endOffset.x() - startOffset.x()), (startOffset.y() - endOffset.y())) * (double)180.0 / (double)M_PI) - angle;
328 : }
329 :
330 :
331 : double
332 50160 : GeomHelper::calculateLotSpaceSlope(const PositionVector& shape, const int index, const double spaceDim) {
333 50160 : return shape.slopeDegreeAtOffset(spaceDim * (index + 1));
334 : }
335 :
336 : /****************************************************************************/
|
