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>
29 #include <utils/common/StdDefs.h>
30 #include <utils/common/ToString.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 GeomHelper::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 
81 double
82 GeomHelper::angle2D(const Position& p1, const Position& p2) {
83  return angleDiff(atan2(p1.y(), p1.x()), atan2(p2.y(), p2.x()));
84 }
85 
86 
87 double
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 
114 double
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 
127 Position
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 
152 double
153 GeomHelper::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 
162 double
163 GeomHelper::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 
172 double
173 GeomHelper::getMinAngleDiff(double angle1, double angle2) {
174  return MIN2(getCWAngleDiff(angle1, angle2), getCCWAngleDiff(angle1, angle2));
175 }
176 
177 
178 double
179 GeomHelper::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 
191 double
192 GeomHelper::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 
208 double
209 GeomHelper::fromNaviDegree(const double angle) {
210  return M_PI / 2. - DEG2RAD(angle);
211 }
212 
213 
214 double
215 GeomHelper::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 
236 GeomHelper::makeCircle(const double radius, const Position& center, unsigned int nPoints) {
237  if (nPoints < 3) {
238  WRITE_ERROR("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 
253 GeomHelper::makeRing(const double radius1, const double radius2, const Position& center, unsigned int nPoints) {
254  if (nPoints < 3) {
255  WRITE_ERROR("GeomHelper::makeRing() requires nPoints>=3");
256  }
257  if (radius1 >= radius2) {
258  WRITE_ERROR("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 
280 const Position
281 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  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 
319 double
320 GeomHelper::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 
329 double
330 GeomHelper::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:288
T MIN2(T a, T b)
Definition: StdDefs.h:74
A class that stores a 2D geometrical boundary.
Definition: Boundary.h:39
double ymin() const
Returns minimum y-coordinate.
Definition: Boundary.cpp:129
double xmin() const
Returns minimum x-coordinate.
Definition: Boundary.cpp:117
double ymax() const
Returns maximum y-coordinate.
Definition: Boundary.cpp:135
double xmax() const
Returns maximum x-coordinate.
Definition: Boundary.cpp:123
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:40