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