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-2018 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>
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 Position
117  if (v.intersects(Position(b.xmin(), b.ymin()), Position(b.xmin(), b.ymax()))) {
118  return v.intersectionPosition2D(
119  Position(b.xmin(), b.ymin()),
120  Position(b.xmin(), b.ymax()));
121  }
122  if (v.intersects(Position(b.xmax(), b.ymin()), Position(b.xmax(), b.ymax()))) {
123  return v.intersectionPosition2D(
124  Position(b.xmax(), b.ymin()),
125  Position(b.xmax(), b.ymax()));
126  }
127  if (v.intersects(Position(b.xmin(), b.ymin()), Position(b.xmax(), b.ymin()))) {
128  return v.intersectionPosition2D(
129  Position(b.xmin(), b.ymin()),
130  Position(b.xmax(), b.ymin()));
131  }
132  if (v.intersects(Position(b.xmin(), b.ymax()), Position(b.xmax(), b.ymax()))) {
133  return v.intersectionPosition2D(
134  Position(b.xmin(), b.ymax()),
135  Position(b.xmax(), b.ymax()));
136  }
137  throw 1;
138 }
139 
140 double
141 GeomHelper::getCCWAngleDiff(double angle1, double angle2) {
142  double v = angle2 - angle1;
143  if (v < 0) {
144  v = 360 + v;
145  }
146  return v;
147 }
148 
149 
150 double
151 GeomHelper::getCWAngleDiff(double angle1, double angle2) {
152  double v = angle1 - angle2;
153  if (v < 0) {
154  v = 360 + v;
155  }
156  return v;
157 }
158 
159 
160 double
161 GeomHelper::getMinAngleDiff(double angle1, double angle2) {
162  return MIN2(getCWAngleDiff(angle1, angle2), getCCWAngleDiff(angle1, angle2));
163 }
164 
165 
166 double
167 GeomHelper::angleDiff(const double angle1, const double angle2) {
168  double dtheta = angle2 - angle1;
169  while (dtheta > (double) M_PI) {
170  dtheta -= (double)(2.0 * M_PI);
171  }
172  while (dtheta < (double) - M_PI) {
173  dtheta += (double)(2.0 * M_PI);
174  }
175  return dtheta;
176 }
177 
178 
179 double
180 GeomHelper::naviDegree(const double angle) {
181  double degree = RAD2DEG(M_PI / 2. - angle);
182  if (std::isinf(degree)) {
183  //assert(false);
184  return 0;
185  }
186  while (degree >= 360.) {
187  degree -= 360.;
188  }
189  while (degree < 0.) {
190  degree += 360.;
191  }
192  return degree;
193 }
194 
195 
196 double
197 GeomHelper::fromNaviDegree(const double angle) {
198  return M_PI / 2. - DEG2RAD(angle);
199 }
200 
201 
202 double
203 GeomHelper::legacyDegree(const double angle, const bool positive) {
204  double degree = -RAD2DEG(M_PI / 2. + angle);
205  if (positive) {
206  while (degree >= 360.) {
207  degree -= 360.;
208  }
209  while (degree < 0.) {
210  degree += 360.;
211  }
212  } else {
213  while (degree >= 180.) {
214  degree -= 360.;
215  }
216  while (degree < -180.) {
217  degree += 360.;
218  }
219  }
220  return degree;
221 }
222 
223 
224 /****************************************************************************/
225 
static double getMinAngleDiff(double angle1, double angle2)
Returns the minimum distance (clockwise/counter-clockwise) between both angles.
Definition: GeomHelper.cpp:161
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:141
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:116
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:203
static double naviDegree(const double angle)
Definition: GeomHelper.cpp:180
static double nearest_offset_on_line_to_point2D(const Position &lineStart, const Position &lineEnd, const Position &p, bool perpendicular=true)
Definition: GeomHelper.cpp:89
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:151
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
static double fromNaviDegree(const double angle)
Definition: GeomHelper.cpp:197
T MIN2(T a, T b)
Definition: StdDefs.h:70
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
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
#define M_PI
Definition: odrSpiral.cpp:40
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:167
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.