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-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 /****************************************************************************/
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>
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
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
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
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
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
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
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
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
175 GeomHelper::getMinAngleDiff(double angle1, double angle2) {
176  return MIN2(getCWAngleDiff(angle1, angle2), getCCWAngleDiff(angle1, angle2));
177 }
178 
179 
180 double
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
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
211 GeomHelper::fromNaviDegree(const double angle) {
212  return M_PI / 2. - DEG2RAD(angle);
213 }
214 
215 
216 double
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 
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 
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 
static double getMinAngleDiff(double angle1, double angle2)
Returns the minimum distance (clockwise/counter-clockwise) between both angles.
Definition: GeomHelper.cpp:175
double ymin() const
Returns minimum y-coordinate.
Definition: Boundary.cpp:131
double xmax() const
Returns maximum x-coordinate.
Definition: Boundary.cpp:125
double distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
Definition: Position.h:244
Position intersectionPosition2D(const Position &p1, const Position &p2, const double withinDist=0.) const
Returns the position of the intersection.
double y() const
Returns the y-position.
Definition: Position.h:62
static double getCCWAngleDiff(double angle1, double angle2)
Returns the distance of second angle from first angle counter-clockwise.
Definition: GeomHelper.cpp:155
static PositionVector makeRing(const double radius1, const double radius2, const Position &center, unsigned int nPoints)
Definition: GeomHelper.cpp:255
double x() const
Returns the x-position.
Definition: Position.h:57
#define RAD2DEG(x)
Definition: GeomHelper.h:39
static Position crossPoint(const Boundary &b, const PositionVector &v)
Definition: GeomHelper.cpp:130
static double nearest_offset_on_line_to_point25D(const Position &lineStart, const Position &lineEnd, const Position &p, bool perpendicular=true)
Definition: GeomHelper.cpp:117
A class that stores a 2D geometrical boundary.
Definition: Boundary.h:42
static double legacyDegree(const double angle, const bool positive=false)
Definition: GeomHelper.cpp:217
static double naviDegree(const double angle)
Definition: GeomHelper.cpp:194
static double nearest_offset_on_line_to_point2D(const Position &lineStart, const Position &lineEnd, const Position &p, bool perpendicular=true)
Definition: GeomHelper.cpp:90
A point in 2D or 3D with translation and scaling methods.
Definition: Position.h:39
A list of positions.
static double getCWAngleDiff(double angle1, double angle2)
Returns the distance of second angle from first angle clockwise.
Definition: GeomHelper.cpp:165
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:84
static double fromNaviDegree(const double angle)
Definition: GeomHelper.cpp:211
static PositionVector makeCircle(const double radius, const Position &center, unsigned int nPoints)
Definition: GeomHelper.cpp:238
T MIN2(T a, T b)
Definition: StdDefs.h:74
double xmin() const
Returns minimum x-coordinate.
Definition: Boundary.cpp:119
#define DEG2RAD(x)
Definition: GeomHelper.h:38
static const double INVALID_OFFSET
a value to signify offsets outside the range of [0, Line.length()]
Definition: GeomHelper.h:52
#define WRITE_ERROR(msg)
Definition: MsgHandler.h:245
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:48
#define M_PI
Definition: odrSpiral.cpp:40
double distanceTo(const Position &p2) const
returns the euclidean distance in 3 dimension
Definition: Position.h:234
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:181
void add(double xoff, double yoff, double zoff)
double ymax() const
Returns maximum y-coordinate.
Definition: Boundary.cpp:137
bool intersects(const Position &p1, const Position &p2) const
Returns the information whether this list of points interesects the given line.