1 /****************************************************************************/
2 // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3 // Copyright (C) 2001-2019 German Aerospace Center (DLR) and others.
4 // This program and the accompanying materials
5 // are made available under the terms of the Eclipse Public License v2.0
6 // which accompanies this distribution, and is available at
7 // http://www.eclipse.org/legal/epl-v20.html
8 // SPDX-License-Identifier: EPL-2.0
9 /****************************************************************************/
10 /// @file GeomHelper.cpp
11 /// @author Daniel Krajzewicz
12 /// @author Friedemann Wesner
13 /// @author Jakob Erdmann
14 /// @author Michael Behrisch
15 /// @date Sept 2002
16 /// @version $Id$
17 ///
18 // Some static methods performing geometrical operations
19 /****************************************************************************/
20
21
22 // ===========================================================================
23 // included modules
24 // ===========================================================================
25 #include <config.h>
26
27 #include <cmath>
28 #include <limits>
29 #include <algorithm>
30 #include <iostream>
31 #include <utils/common/StdDefs.h>
32 #include <utils/common/ToString.h>
33 #include <utils/common/MsgHandler.h>
34 #include "Boundary.h"
35 #include "GeomHelper.h"
36
37 // ===========================================================================
38 // static members
39 // ===========================================================================
40 const double GeomHelper::INVALID_OFFSET = -1;
41
42
43 // ===========================================================================
44 // method definitions
45 // ===========================================================================
46
47 void
findLineCircleIntersections(const Position & c,double radius,const Position & p1,const Position & p2,std::vector<double> & into)48 GeomHelper::findLineCircleIntersections(const Position& c, double radius, const Position& p1, const Position& p2,
49 std::vector<double>& into) {
50 const double dx = p2.x() - p1.x();
51 const double dy = p2.y() - p1.y();
52
53 const double A = dx * dx + dy * dy;
54 const double B = 2 * (dx * (p1.x() - c.x()) + dy * (p1.y() - c.y()));
55 const double C = (p1.x() - c.x()) * (p1.x() - c.x()) + (p1.y() - c.y()) * (p1.y() - c.y()) - radius * radius;
56
57 const double det = B * B - 4 * A * C;
58 if ((A <= 0.0000001) || (det < 0)) {
59 // No real solutions.
60 return;
61 }
62 if (det == 0) {
63 // One solution.
64 const double t = -B / (2 * A);
65 if (t >= 0. && t <= 1.) {
66 into.push_back(t);
67 }
68 } else {
69 // Two solutions.
70 const double t = (double)((-B + sqrt(det)) / (2 * A));
71 Position intersection(p1.x() + t * dx, p1.y() + t * dy);
72 if (t >= 0. && t <= 1.) {
73 into.push_back(t);
74 }
75 const double t2 = (double)((-B - sqrt(det)) / (2 * A));
76 if (t2 >= 0. && t2 <= 1.) {
77 into.push_back(t2);
78 }
79 }
80 }
81
82
83 double
angle2D(const Position & p1,const Position & p2)84 GeomHelper::angle2D(const Position& p1, const Position& p2) {
85 return angleDiff(atan2(p1.y(), p1.x()), atan2(p2.y(), p2.x()));
86 }
87
88
89 double
nearest_offset_on_line_to_point2D(const Position & lineStart,const Position & lineEnd,const Position & p,bool perpendicular)90 GeomHelper::nearest_offset_on_line_to_point2D(const Position& lineStart,
91 const Position& lineEnd,
92 const Position& p, bool perpendicular) {
93 const double lineLength2D = lineStart.distanceTo2D(lineEnd);
94 if (lineLength2D == 0.0f) {
95 return 0.0f;
96 } else {
97 // scalar product equals length of orthogonal projection times length of vector being projected onto
98 // dividing the scalar product by the square of the distance gives the relative position
99 const double u = (((p.x() - lineStart.x()) * (lineEnd.x() - lineStart.x())) +
100 ((p.y() - lineStart.y()) * (lineEnd.y() - lineStart.y()))
101 ) / (lineLength2D * lineLength2D);
102 if (u < 0.0f || u > 1.0f) { // closest point does not fall within the line segment
103 if (perpendicular) {
104 return INVALID_OFFSET;
105 }
106 if (u < 0.0f) {
107 return 0.0f;
108 }
109 return lineLength2D;
110 }
111 return u * lineLength2D;
112 }
113 }
114
115
116 double
nearest_offset_on_line_to_point25D(const Position & lineStart,const Position & lineEnd,const Position & p,bool perpendicular)117 GeomHelper::nearest_offset_on_line_to_point25D(const Position& lineStart,
118 const Position& lineEnd,
119 const Position& p, bool perpendicular) {
120 double result = nearest_offset_on_line_to_point2D(lineStart, lineEnd, p, perpendicular);
121 if (result != INVALID_OFFSET) {
122 const double lineLength2D = lineStart.distanceTo2D(lineEnd);
123 const double lineLength = lineStart.distanceTo(lineEnd);
124 result *= (lineLength / lineLength2D);
125 }
126 return result;
127 }
128
129 Position
crossPoint(const Boundary & b,const PositionVector & v)130 GeomHelper::crossPoint(const Boundary& b, const PositionVector& v) {
131 if (v.intersects(Position(b.xmin(), b.ymin()), Position(b.xmin(), b.ymax()))) {
132 return v.intersectionPosition2D(
133 Position(b.xmin(), b.ymin()),
134 Position(b.xmin(), b.ymax()));
135 }
136 if (v.intersects(Position(b.xmax(), b.ymin()), Position(b.xmax(), b.ymax()))) {
137 return v.intersectionPosition2D(
138 Position(b.xmax(), b.ymin()),
139 Position(b.xmax(), b.ymax()));
140 }
141 if (v.intersects(Position(b.xmin(), b.ymin()), Position(b.xmax(), b.ymin()))) {
142 return v.intersectionPosition2D(
143 Position(b.xmin(), b.ymin()),
144 Position(b.xmax(), b.ymin()));
145 }
146 if (v.intersects(Position(b.xmin(), b.ymax()), Position(b.xmax(), b.ymax()))) {
147 return v.intersectionPosition2D(
148 Position(b.xmin(), b.ymax()),
149 Position(b.xmax(), b.ymax()));
150 }
151 throw 1;
152 }
153
154 double
getCCWAngleDiff(double angle1,double angle2)155 GeomHelper::getCCWAngleDiff(double angle1, double angle2) {
156 double v = angle2 - angle1;
157 if (v < 0) {
158 v = 360 + v;
159 }
160 return v;
161 }
162
163
164 double
getCWAngleDiff(double angle1,double angle2)165 GeomHelper::getCWAngleDiff(double angle1, double angle2) {
166 double v = angle1 - angle2;
167 if (v < 0) {
168 v = 360 + v;
169 }
170 return v;
171 }
172
173
174 double
getMinAngleDiff(double angle1,double angle2)175 GeomHelper::getMinAngleDiff(double angle1, double angle2) {
176 return MIN2(getCWAngleDiff(angle1, angle2), getCCWAngleDiff(angle1, angle2));
177 }
178
179
180 double
angleDiff(const double angle1,const double angle2)181 GeomHelper::angleDiff(const double angle1, const double angle2) {
182 double dtheta = angle2 - angle1;
183 while (dtheta > (double) M_PI) {
184 dtheta -= (double)(2.0 * M_PI);
185 }
186 while (dtheta < (double) - M_PI) {
187 dtheta += (double)(2.0 * M_PI);
188 }
189 return dtheta;
190 }
191
192
193 double
naviDegree(const double angle)194 GeomHelper::naviDegree(const double angle) {
195 double degree = RAD2DEG(M_PI / 2. - angle);
196 if (std::isinf(degree)) {
197 //assert(false);
198 return 0;
199 }
200 while (degree >= 360.) {
201 degree -= 360.;
202 }
203 while (degree < 0.) {
204 degree += 360.;
205 }
206 return degree;
207 }
208
209
210 double
fromNaviDegree(const double angle)211 GeomHelper::fromNaviDegree(const double angle) {
212 return M_PI / 2. - DEG2RAD(angle);
213 }
214
215
216 double
legacyDegree(const double angle,const bool positive)217 GeomHelper::legacyDegree(const double angle, const bool positive) {
218 double degree = -RAD2DEG(M_PI / 2. + angle);
219 if (positive) {
220 while (degree >= 360.) {
221 degree -= 360.;
222 }
223 while (degree < 0.) {
224 degree += 360.;
225 }
226 } else {
227 while (degree >= 180.) {
228 degree -= 360.;
229 }
230 while (degree < -180.) {
231 degree += 360.;
232 }
233 }
234 return degree;
235 }
236
237 PositionVector
makeCircle(const double radius,const Position & center,unsigned int nPoints)238 GeomHelper::makeCircle(const double radius, const Position& center, unsigned int nPoints) {
239 if (nPoints < 3) {
240 WRITE_ERROR("GeomHelper::makeCircle() requires nPoints>=3");
241 }
242 PositionVector circle;
243 circle.push_back({radius, 0});
244 for (unsigned int i = 1; i < nPoints; ++i) {
245 const double a = 2.0 * M_PI * (double)i / (double) nPoints;
246 circle.push_back({radius * cos(a), radius * sin(a)});
247 }
248 circle.push_back({radius, 0});
249 circle.add(center);
250 return circle;
251 }
252
253
254 PositionVector
makeRing(const double radius1,const double radius2,const Position & center,unsigned int nPoints)255 GeomHelper::makeRing(const double radius1, const double radius2, const Position& center, unsigned int nPoints) {
256 if (nPoints < 3) {
257 WRITE_ERROR("GeomHelper::makeRing() requires nPoints>=3");
258 }
259 if (radius1 >= radius2) {
260 WRITE_ERROR("GeomHelper::makeRing() requires radius2>radius1");
261 }
262 PositionVector ring;
263 ring.push_back({radius1, 0});
264 ring.push_back({radius2, 0});
265 for (unsigned int i = 1; i < nPoints; ++i) {
266 const double a = 2.0 * M_PI * (double)i / (double) nPoints;
267 ring.push_back({radius2 * cos(a), radius2 * sin(a)});
268 }
269 ring.push_back({radius2, 0});
270 ring.push_back({radius1, 0});
271 for (unsigned int i = 1; i < nPoints; ++i) {
272 const double a = -2.0 * M_PI * (double)i / (double) nPoints;
273 ring.push_back({radius1 * cos(a), radius1 * sin(a)});
274 }
275 ring.push_back({radius1, 0});
276 ring.add(center);
277 return ring;
278 }
279
280 /****************************************************************************/
281
282