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 174711201 : GeomHelper::angle2D(const Position& p1, const Position& p2) {
83 174711201 : return angleDiff(atan2(p1.y(), p1.x()), atan2(p2.y(), p2.x()));
84 : }
85 :
86 :
87 : double
88 73522125 : 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 73522125 : 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 73329938 : const double u = (((p.x() - lineStart.x()) * (lineEnd.x() - lineStart.x())) +
98 73329938 : ((p.y() - lineStart.y()) * (lineEnd.y() - lineStart.y()))
99 73329938 : ) / lineLength2D;
100 73329938 : if (u < 0. || u > lineLength2D) { // closest point does not fall within the line segment
101 56020190 : if (perpendicular) {
102 : return INVALID_OFFSET;
103 : }
104 49473241 : if (u < 0.) {
105 : return 0.;
106 : }
107 20326674 : 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 1163550 : GeomHelper::getCCWAngleDiff(double angle1, double angle2) {
153 1163550 : double v = angle2 - angle1;
154 1163550 : if (v < 0) {
155 559383 : v = 360 + v;
156 : }
157 1163550 : return v;
158 : }
159 :
160 :
161 : double
162 1173802 : GeomHelper::getCWAngleDiff(double angle1, double angle2) {
163 1173802 : double v = angle1 - angle2;
164 1173802 : if (v < 0) {
165 548121 : v = 360 + v;
166 : }
167 1173802 : return v;
168 : }
169 :
170 :
171 : double
172 1158761 : GeomHelper::getMinAngleDiff(double angle1, double angle2) {
173 1158761 : return MIN2(getCWAngleDiff(angle1, angle2), getCCWAngleDiff(angle1, angle2));
174 : }
175 :
176 :
177 : double
178 182985435 : GeomHelper::angleDiff(const double angle1, const double angle2) {
179 182985435 : double dtheta = angle2 - angle1;
180 185766311 : while (dtheta > (double) M_PI) {
181 2780876 : dtheta -= (double)(2.0 * M_PI);
182 : }
183 185345696 : while (dtheta < (double) - M_PI) {
184 2360261 : dtheta += (double)(2.0 * M_PI);
185 : }
186 182985435 : return dtheta;
187 : }
188 :
189 :
190 : double
191 3710862 : GeomHelper::naviDegree(const double angle) {
192 3710862 : double degree = RAD2DEG(M_PI / 2. - angle);
193 3710862 : if (std::isinf(degree)) {
194 : //assert(false);
195 : return 0;
196 : }
197 3710862 : while (degree >= 360.) {
198 0 : degree -= 360.;
199 : }
200 4401284 : while (degree < 0.) {
201 690422 : degree += 360.;
202 : }
203 : return degree;
204 : }
205 :
206 :
207 : double
208 29275 : GeomHelper::fromNaviDegree(const double angle) {
209 29275 : return M_PI / 2. - DEG2RAD(angle);
210 : }
211 :
212 :
213 : double
214 28514458 : GeomHelper::legacyDegree(const double angle, const bool positive) {
215 28514458 : double degree = -RAD2DEG(M_PI / 2. + angle);
216 28514458 : if (positive) {
217 3073113 : while (degree >= 360.) {
218 0 : degree -= 360.;
219 : }
220 5403822 : while (degree < 0.) {
221 2330709 : degree += 360.;
222 : }
223 : } else {
224 25441345 : while (degree >= 180.) {
225 0 : degree -= 360.;
226 : }
227 31823095 : while (degree < -180.) {
228 6381750 : degree += 360.;
229 : }
230 : }
231 28514458 : 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 86 : GeomHelper::makeRing(const double radius1, const double radius2, const Position& center, unsigned int nPoints) {
253 86 : if (nPoints < 3) {
254 0 : WRITE_ERROR("GeomHelper::makeRing() requires nPoints>=3");
255 : }
256 86 : if (radius1 >= radius2) {
257 0 : WRITE_ERROR("GeomHelper::makeRing() requires radius2>radius1");
258 : }
259 86 : PositionVector ring;
260 86 : ring.push_back({radius1, 0});
261 86 : ring.push_back({radius2, 0});
262 2924 : for (unsigned int i = 1; i < nPoints; ++i) {
263 2838 : const double a = 2.0 * M_PI * (double)i / (double) nPoints;
264 2838 : ring.push_back({radius2 * cos(a), radius2 * sin(a)});
265 : }
266 86 : ring.push_back({radius2, 0});
267 86 : ring.push_back({radius1, 0});
268 2924 : for (unsigned int i = 1; i < nPoints; ++i) {
269 2838 : const double a = -2.0 * M_PI * (double)i / (double) nPoints;
270 2838 : ring.push_back({radius1 * cos(a), radius1 * sin(a)});
271 : }
272 86 : ring.push_back({radius1, 0});
273 86 : ring.add(center);
274 86 : return ring;
275 0 : }
276 :
277 :
278 :
279 : const Position
280 37749 : 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 37749 : const Position startOffset = shape.positionAtOffset(spaceDim * (index));
286 37749 : const Position endOffset = shape.positionAtOffset(spaceDim * (index + 1));
287 : // continue depending of nagle
288 37749 : if (angle == 0) {
289 : // parking parallel to the road
290 36419 : pos = endOffset;
291 : } else {
292 : // angled parking
293 1330 : const double hlp_angle = fabs(((double)atan2((endOffset.x() - startOffset.x()), (startOffset.y() - endOffset.y())) * (double)180.0 / (double)M_PI) - 180);
294 1330 : if (angle >= 0 && angle <= 90) {
295 585 : pos.setx((startOffset.x() + endOffset.x()) / 2 - (width / 2) * (1 - cos(angle / 180 * M_PI)) * cos(hlp_angle / 180 * M_PI));
296 585 : pos.sety((startOffset.y() + endOffset.y()) / 2 + (width / 2) * (1 - cos(angle / 180 * M_PI)) * sin(hlp_angle / 180 * M_PI));
297 585 : pos.setz((startOffset.z() + endOffset.z()) / 2);
298 745 : } 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 745 : } else if (angle > 180 && angle <= 270) {
303 640 : 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 640 : 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 640 : pos.setz((startOffset.z() + endOffset.z()) / 2);
306 105 : } 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 37749 : return pos;
315 : }
316 :
317 :
318 : double
319 37749 : GeomHelper::calculateLotSpaceAngle(const PositionVector& shape, const int index, const double spaceDim, const double angle) {
320 : // declare shape offsets
321 37749 : const Position startOffset = shape.positionAtOffset(spaceDim * (index));
322 37749 : const Position endOffset = shape.positionAtOffset(spaceDim * (index + 1));
323 : // return angle
324 37749 : return ((double)atan2((endOffset.x() - startOffset.x()), (startOffset.y() - endOffset.y())) * (double)180.0 / (double)M_PI) + angle;
325 : }
326 :
327 :
328 : double
329 37749 : GeomHelper::calculateLotSpaceSlope(const PositionVector& shape, const int index, const double spaceDim) {
330 37749 : return shape.slopeDegreeAtOffset(spaceDim * (index + 1));
331 : }
332 :
333 : /****************************************************************************/
|
