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 /****************************************************************************/
17 // Some static methods performing geometrical operations
18 /****************************************************************************/
19 
20 
21 // ===========================================================================
22 // included modules
23 // ===========================================================================
24 #include <config.h>
25 
26 #include <cmath>
27 #include <limits>
28 #include <algorithm>
29 #include <iostream>
30 #include <utils/common/StdDefs.h>
31 #include <utils/common/ToString.h>
33 #include "Boundary.h"
34 #include "GeomHelper.h"
35 
36 // ===========================================================================
37 // static members
38 // ===========================================================================
39 const double GeomHelper::INVALID_OFFSET = -1;
40 
41 
42 // ===========================================================================
43 // method definitions
44 // ===========================================================================
45 
46 void
47 GeomHelper::findLineCircleIntersections(const Position& c, double radius, const Position& p1, const Position& p2,
48  std::vector<double>& into) {
49  const double dx = p2.x() - p1.x();
50  const double dy = p2.y() - p1.y();
51 
52  const double A = dx * dx + dy * dy;
53  const double B = 2 * (dx * (p1.x() - c.x()) + dy * (p1.y() - c.y()));
54  const double C = (p1.x() - c.x()) * (p1.x() - c.x()) + (p1.y() - c.y()) * (p1.y() - c.y()) - radius * radius;
55 
56  const double det = B * B - 4 * A * C;
57  if ((A <= 0.0000001) || (det < 0)) {
58  // No real solutions.
59  return;
60  }
61  if (det == 0) {
62  // One solution.
63  const double t = -B / (2 * A);
64  if (t >= 0. && t <= 1.) {
65  into.push_back(t);
66  }
67  } else {
68  // Two solutions.
69  const double t = (double)((-B + sqrt(det)) / (2 * A));
70  Position intersection(p1.x() + t * dx, p1.y() + t * dy);
71  if (t >= 0. && t <= 1.) {
72  into.push_back(t);
73  }
74  const double t2 = (double)((-B - sqrt(det)) / (2 * A));
75  if (t2 >= 0. && t2 <= 1.) {
76  into.push_back(t2);
77  }
78  }
79 }
80 
81 
82 double
83 GeomHelper::angle2D(const Position& p1, const Position& p2) {
84  return angleDiff(atan2(p1.y(), p1.x()), atan2(p2.y(), p2.x()));
85 }
86 
87 
88 double
90  const Position& lineEnd,
91  const Position& p, bool perpendicular) {
92  const double lineLength2D = lineStart.distanceTo2D(lineEnd);
93  if (lineLength2D == 0.0f) {
94  return 0.0f;
95  } else {
96  // scalar product equals length of orthogonal projection times length of vector being projected onto
97  // dividing the scalar product by the square of the distance gives the relative position
98  const double u = (((p.x() - lineStart.x()) * (lineEnd.x() - lineStart.x())) +
99  ((p.y() - lineStart.y()) * (lineEnd.y() - lineStart.y()))
100  ) / (lineLength2D * lineLength2D);
101  if (u < 0.0f || u > 1.0f) { // closest point does not fall within the line segment
102  if (perpendicular) {
103  return INVALID_OFFSET;
104  }
105  if (u < 0.0f) {
106  return 0.0f;
107  }
108  return lineLength2D;
109  }
110  return u * lineLength2D;
111  }
112 }
113 
114 
115 double
117  const Position& lineEnd,
118  const Position& p, bool perpendicular) {
119  double result = nearest_offset_on_line_to_point2D(lineStart, lineEnd, p, perpendicular);
120  if (result != INVALID_OFFSET) {
121  const double lineLength2D = lineStart.distanceTo2D(lineEnd);
122  const double lineLength = lineStart.distanceTo(lineEnd);
123  result *= (lineLength / lineLength2D);
124  }
125  return result;
126 }
127 
128 Position
130  if (v.intersects(Position(b.xmin(), b.ymin()), Position(b.xmin(), b.ymax()))) {
131  return v.intersectionPosition2D(
132  Position(b.xmin(), b.ymin()),
133  Position(b.xmin(), b.ymax()));
134  }
135  if (v.intersects(Position(b.xmax(), b.ymin()), Position(b.xmax(), b.ymax()))) {
136  return v.intersectionPosition2D(
137  Position(b.xmax(), b.ymin()),
138  Position(b.xmax(), b.ymax()));
139  }
140  if (v.intersects(Position(b.xmin(), b.ymin()), Position(b.xmax(), b.ymin()))) {
141  return v.intersectionPosition2D(
142  Position(b.xmin(), b.ymin()),
143  Position(b.xmax(), b.ymin()));
144  }
145  if (v.intersects(Position(b.xmin(), b.ymax()), Position(b.xmax(), b.ymax()))) {
146  return v.intersectionPosition2D(
147  Position(b.xmin(), b.ymax()),
148  Position(b.xmax(), b.ymax()));
149  }
150  throw 1;
151 }
152 
153 double
154 GeomHelper::getCCWAngleDiff(double angle1, double angle2) {
155  double v = angle2 - angle1;
156  if (v < 0) {
157  v = 360 + v;
158  }
159  return v;
160 }
161 
162 
163 double
164 GeomHelper::getCWAngleDiff(double angle1, double angle2) {
165  double v = angle1 - angle2;
166  if (v < 0) {
167  v = 360 + v;
168  }
169  return v;
170 }
171 
172 
173 double
174 GeomHelper::getMinAngleDiff(double angle1, double angle2) {
175  return MIN2(getCWAngleDiff(angle1, angle2), getCCWAngleDiff(angle1, angle2));
176 }
177 
178 
179 double
180 GeomHelper::angleDiff(const double angle1, const double angle2) {
181  double dtheta = angle2 - angle1;
182  while (dtheta > (double) M_PI) {
183  dtheta -= (double)(2.0 * M_PI);
184  }
185  while (dtheta < (double) - M_PI) {
186  dtheta += (double)(2.0 * M_PI);
187  }
188  return dtheta;
189 }
190 
191 
192 double
193 GeomHelper::naviDegree(const double angle) {
194  double degree = RAD2DEG(M_PI / 2. - angle);
195  if (std::isinf(degree)) {
196  //assert(false);
197  return 0;
198  }
199  while (degree >= 360.) {
200  degree -= 360.;
201  }
202  while (degree < 0.) {
203  degree += 360.;
204  }
205  return degree;
206 }
207 
208 
209 double
210 GeomHelper::fromNaviDegree(const double angle) {
211  return M_PI / 2. - DEG2RAD(angle);
212 }
213 
214 
215 double
216 GeomHelper::legacyDegree(const double angle, const bool positive) {
217  double degree = -RAD2DEG(M_PI / 2. + angle);
218  if (positive) {
219  while (degree >= 360.) {
220  degree -= 360.;
221  }
222  while (degree < 0.) {
223  degree += 360.;
224  }
225  } else {
226  while (degree >= 180.) {
227  degree -= 360.;
228  }
229  while (degree < -180.) {
230  degree += 360.;
231  }
232  }
233  return degree;
234 }
235 
237 GeomHelper::makeCircle(const double radius, const Position& center, unsigned int nPoints) {
238  if (nPoints < 3) {
239  WRITE_ERROR("GeomHelper::makeCircle() requires nPoints>=3");
240  }
241  PositionVector circle;
242  circle.push_back({radius, 0});
243  for (unsigned int i = 1; i < nPoints; ++i) {
244  const double a = 2.0 * M_PI * (double)i / (double) nPoints;
245  circle.push_back({radius * cos(a), radius * sin(a)});
246  }
247  circle.push_back({radius, 0});
248  circle.add(center);
249  return circle;
250 }
251 
252 
254 GeomHelper::makeRing(const double radius1, const double radius2, const Position& center, unsigned int nPoints) {
255  if (nPoints < 3) {
256  WRITE_ERROR("GeomHelper::makeRing() requires nPoints>=3");
257  }
258  if (radius1 >= radius2) {
259  WRITE_ERROR("GeomHelper::makeRing() requires radius2>radius1");
260  }
261  PositionVector ring;
262  ring.push_back({radius1, 0});
263  ring.push_back({radius2, 0});
264  for (unsigned int i = 1; i < nPoints; ++i) {
265  const double a = 2.0 * M_PI * (double)i / (double) nPoints;
266  ring.push_back({radius2 * cos(a), radius2 * sin(a)});
267  }
268  ring.push_back({radius2, 0});
269  ring.push_back({radius1, 0});
270  for (unsigned int i = 1; i < nPoints; ++i) {
271  const double a = -2.0 * M_PI * (double)i / (double) nPoints;
272  ring.push_back({radius1 * cos(a), radius1 * sin(a)});
273  }
274  ring.push_back({radius1, 0});
275  ring.add(center);
276  return ring;
277 }
278 
279 /****************************************************************************/
280 
Boundary.h
ToString.h
MIN2
T MIN2(T a, T b)
Definition: StdDefs.h:73
GeomHelper::angleDiff
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:180
Boundary::ymin
double ymin() const
Returns minimum y-coordinate.
Definition: Boundary.cpp:130
GeomHelper::getMinAngleDiff
static double getMinAngleDiff(double angle1, double angle2)
Returns the minimum distance (clockwise/counter-clockwise) between both angles.
Definition: GeomHelper.cpp:174
MsgHandler.h
GeomHelper::nearest_offset_on_line_to_point25D
static double nearest_offset_on_line_to_point25D(const Position &lineStart, const Position &lineEnd, const Position &p, bool perpendicular=true)
Definition: GeomHelper.cpp:116
GeomHelper::legacyDegree
static double legacyDegree(const double angle, const bool positive=false)
Definition: GeomHelper.cpp:216
GeomHelper::naviDegree
static double naviDegree(const double angle)
Definition: GeomHelper.cpp:193
Boundary::xmax
double xmax() const
Returns maximum x-coordinate.
Definition: Boundary.cpp:124
RAD2DEG
#define RAD2DEG(x)
Definition: GeomHelper.h:38
GeomHelper::nearest_offset_on_line_to_point2D
static double nearest_offset_on_line_to_point2D(const Position &lineStart, const Position &lineEnd, const Position &p, bool perpendicular=true)
Definition: GeomHelper.cpp:89
PositionVector
A list of positions.
Definition: PositionVector.h:45
GeomHelper::makeRing
static PositionVector makeRing(const double radius1, const double radius2, const Position &center, unsigned int nPoints)
Definition: GeomHelper.cpp:254
PositionVector::add
void add(double xoff, double yoff, double zoff)
Definition: PositionVector.cpp:617
PositionVector::intersects
bool intersects(const Position &p1, const Position &p2) const
Returns the information whether this list of points interesects the given line.
Definition: PositionVector.cpp:159
Boundary::xmin
double xmin() const
Returns minimum x-coordinate.
Definition: Boundary.cpp:118
GeomHelper::INVALID_OFFSET
static const double INVALID_OFFSET
a value to signify offsets outside the range of [0, Line.length()]
Definition: GeomHelper.h:51
GeomHelper::crossPoint
static Position crossPoint(const Boundary &b, const PositionVector &v)
Definition: GeomHelper.cpp:129
Position::distanceTo
double distanceTo(const Position &p2) const
returns the euclidean distance in 3 dimension
Definition: Position.h:233
GeomHelper::findLineCircleIntersections
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:47
Boundary
A class that stores a 2D geometrical boundary.
Definition: Boundary.h:41
Position
A point in 2D or 3D with translation and scaling methods.
Definition: Position.h:38
Position::x
double x() const
Returns the x-position.
Definition: Position.h:56
DEG2RAD
#define DEG2RAD(x)
Definition: GeomHelper.h:37
Position::distanceTo2D
double distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
Definition: Position.h:243
GeomHelper::getCWAngleDiff
static double getCWAngleDiff(double angle1, double angle2)
Returns the distance of second angle from first angle clockwise.
Definition: GeomHelper.cpp:164
GeomHelper::angle2D
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:83
Position::y
double y() const
Returns the y-position.
Definition: Position.h:61
M_PI
#define M_PI
Definition: odrSpiral.cpp:40
PositionVector::intersectionPosition2D
Position intersectionPosition2D(const Position &p1, const Position &p2, const double withinDist=0.) const
Returns the position of the intersection.
Definition: PositionVector.cpp:187
GeomHelper::fromNaviDegree
static double fromNaviDegree(const double angle)
Definition: GeomHelper.cpp:210
GeomHelper::makeCircle
static PositionVector makeCircle(const double radius, const Position &center, unsigned int nPoints)
Definition: GeomHelper.cpp:237
config.h
GeomHelper.h
StdDefs.h
WRITE_ERROR
#define WRITE_ERROR(msg)
Definition: MsgHandler.h:283
GeomHelper::getCCWAngleDiff
static double getCCWAngleDiff(double angle1, double angle2)
Returns the distance of second angle from first angle counter-clockwise.
Definition: GeomHelper.cpp:154
Boundary::ymax
double ymax() const
Returns maximum y-coordinate.
Definition: Boundary.cpp:136