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-2017 German Aerospace Center (DLR) and others.
4 /****************************************************************************/
5 //
6 // This program and the accompanying materials
7 // are made available under the terms of the Eclipse Public License v2.0
8 // which accompanies this distribution, and is available at
9 // http://www.eclipse.org/legal/epl-v20.html
10 //
11 /****************************************************************************/
20 // Some static methods performing geometrical operations
21 /****************************************************************************/
22 
23 
24 // ===========================================================================
25 // included modules
26 // ===========================================================================
27 #ifdef _MSC_VER
28 #include <windows_config.h>
29 #else
30 #include <config.h>
31 #endif
32 
33 #include <cmath>
34 #include <limits>
35 #include <algorithm>
36 #include <iostream>
37 #include <utils/common/StdDefs.h>
38 #include <utils/common/ToString.h>
39 #include "Boundary.h"
40 #include "GeomHelper.h"
41 
42 // ===========================================================================
43 // static members
44 // ===========================================================================
45 const double GeomHelper::INVALID_OFFSET = -1;
46 
47 
48 // ===========================================================================
49 // method definitions
50 // ===========================================================================
51 
52 void
53 GeomHelper::findLineCircleIntersections(const Position& c, double radius, const Position& p1, const Position& p2,
54  std::vector<double>& into) {
55  const double dx = p2.x() - p1.x();
56  const double dy = p2.y() - p1.y();
57 
58  const double A = dx * dx + dy * dy;
59  const double B = 2 * (dx * (p1.x() - c.x()) + dy * (p1.y() - c.y()));
60  const double C = (p1.x() - c.x()) * (p1.x() - c.x()) + (p1.y() - c.y()) * (p1.y() - c.y()) - radius * radius;
61 
62  const double det = B * B - 4 * A * C;
63  if ((A <= 0.0000001) || (det < 0)) {
64  // No real solutions.
65  return;
66  }
67  if (det == 0) {
68  // One solution.
69  const double t = -B / (2 * A);
70  if (t >= 0. && t <= 1.) {
71  into.push_back(t);
72  }
73  } else {
74  // Two solutions.
75  const double t = (double)((-B + sqrt(det)) / (2 * A));
76  Position intersection(p1.x() + t * dx, p1.y() + t * dy);
77  if (t >= 0. && t <= 1.) {
78  into.push_back(t);
79  }
80  const double t2 = (double)((-B - sqrt(det)) / (2 * A));
81  if (t2 >= 0. && t2 <= 1.) {
82  into.push_back(t2);
83  }
84  }
85 }
86 
87 
88 double
89 GeomHelper::angle2D(const Position& p1, const Position& p2) {
90  return angleDiff(atan2(p1.y(), p1.x()), atan2(p2.y(), p2.x()));
91 }
92 
93 
94 double
96  const Position& lineEnd,
97  const Position& p, bool perpendicular) {
98  const double lineLength2D = lineStart.distanceTo2D(lineEnd);
99  if (lineLength2D == 0.0f) {
100  return 0.0f;
101  } else {
102  // scalar product equals length of orthogonal projection times length of vector being projected onto
103  // dividing the scalar product by the square of the distance gives the relative position
104  const double u = (((p.x() - lineStart.x()) * (lineEnd.x() - lineStart.x())) +
105  ((p.y() - lineStart.y()) * (lineEnd.y() - lineStart.y()))
106  ) / (lineLength2D * lineLength2D);
107  if (u < 0.0f || u > 1.0f) { // closest point does not fall within the line segment
108  if (perpendicular) {
109  return INVALID_OFFSET;
110  }
111  if (u < 0.0f) {
112  return 0.0f;
113  }
114  return lineLength2D;
115  }
116  return u * lineLength2D;
117  }
118 }
119 
120 
121 Position
123  if (v.intersects(Position(b.xmin(), b.ymin()), Position(b.xmin(), b.ymax()))) {
124  return v.intersectionPosition2D(
125  Position(b.xmin(), b.ymin()),
126  Position(b.xmin(), b.ymax()));
127  }
128  if (v.intersects(Position(b.xmax(), b.ymin()), Position(b.xmax(), b.ymax()))) {
129  return v.intersectionPosition2D(
130  Position(b.xmax(), b.ymin()),
131  Position(b.xmax(), b.ymax()));
132  }
133  if (v.intersects(Position(b.xmin(), b.ymin()), Position(b.xmax(), b.ymin()))) {
134  return v.intersectionPosition2D(
135  Position(b.xmin(), b.ymin()),
136  Position(b.xmax(), b.ymin()));
137  }
138  if (v.intersects(Position(b.xmin(), b.ymax()), Position(b.xmax(), b.ymax()))) {
139  return v.intersectionPosition2D(
140  Position(b.xmin(), b.ymax()),
141  Position(b.xmax(), b.ymax()));
142  }
143  throw 1;
144 }
145 
146 double
147 GeomHelper::getCCWAngleDiff(double angle1, double angle2) {
148  double v = angle2 - angle1;
149  if (v < 0) {
150  v = 360 + v;
151  }
152  return v;
153 }
154 
155 
156 double
157 GeomHelper::getCWAngleDiff(double angle1, double angle2) {
158  double v = angle1 - angle2;
159  if (v < 0) {
160  v = 360 + v;
161  }
162  return v;
163 }
164 
165 
166 double
167 GeomHelper::getMinAngleDiff(double angle1, double angle2) {
168  return MIN2(getCWAngleDiff(angle1, angle2), getCCWAngleDiff(angle1, angle2));
169 }
170 
171 
172 double
173 GeomHelper::angleDiff(const double angle1, const double angle2) {
174  double dtheta = angle2 - angle1;
175  while (dtheta > (double) M_PI) {
176  dtheta -= (double)(2.0 * M_PI);
177  }
178  while (dtheta < (double) - M_PI) {
179  dtheta += (double)(2.0 * M_PI);
180  }
181  return dtheta;
182 }
183 
184 
185 double
186 GeomHelper::naviDegree(const double angle) {
187  double degree = RAD2DEG(M_PI / 2. - angle);
188  while (degree >= 360.) {
189  degree -= 360.;
190  }
191  while (degree < 0.) {
192  degree += 360.;
193  }
194  return degree;
195 }
196 
197 
198 double
199 GeomHelper::fromNaviDegree(const double angle) {
200  return M_PI / 2. - DEG2RAD(angle);
201 }
202 
203 
204 double
205 GeomHelper::legacyDegree(const double angle, const bool positive) {
206  double degree = -RAD2DEG(M_PI / 2. + angle);
207  if (positive) {
208  while (degree >= 360.) {
209  degree -= 360.;
210  }
211  while (degree < 0.) {
212  degree += 360.;
213  }
214  } else {
215  while (degree >= 180.) {
216  degree -= 360.;
217  }
218  while (degree < -180.) {
219  degree += 360.;
220  }
221  }
222  return degree;
223 }
224 
225 
226 /****************************************************************************/
227 
static double getMinAngleDiff(double angle1, double angle2)
Returns the minimum distance (clockwise/counter-clockwise) between both angles.
Definition: GeomHelper.cpp:167
double ymin() const
Returns minimum y-coordinate.
Definition: Boundary.cpp:137
double xmax() const
Returns maximum x-coordinate.
Definition: Boundary.cpp:131
double distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
Definition: Position.h:249
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:67
static double getCCWAngleDiff(double angle1, double angle2)
Returns the distance of second angle from first angle counter-clockwise.
Definition: GeomHelper.cpp:147
double x() const
Returns the x-position.
Definition: Position.h:62
#define RAD2DEG(x)
Definition: GeomHelper.h:45
static Position crossPoint(const Boundary &b, const PositionVector &v)
Definition: GeomHelper.cpp:122
A class that stores a 2D geometrical boundary.
Definition: Boundary.h:47
static double legacyDegree(const double angle, const bool positive=false)
Definition: GeomHelper.cpp:205
static double naviDegree(const double angle)
Definition: GeomHelper.cpp:186
static double nearest_offset_on_line_to_point2D(const Position &lineStart, const Position &lineEnd, const Position &p, bool perpendicular=true)
Definition: GeomHelper.cpp:95
A point in 2D or 3D with translation and scaling methods.
Definition: Position.h:45
A list of positions.
static double getCWAngleDiff(double angle1, double angle2)
Returns the distance of second angle from first angle clockwise.
Definition: GeomHelper.cpp:157
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:89
static double fromNaviDegree(const double angle)
Definition: GeomHelper.cpp:199
T MIN2(T a, T b)
Definition: StdDefs.h:67
double xmin() const
Returns minimum x-coordinate.
Definition: Boundary.cpp:125
#define DEG2RAD(x)
Definition: GeomHelper.h:44
static const double INVALID_OFFSET
a value to signify offsets outside the range of [0, Line.length()]
Definition: GeomHelper.h:58
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:53
#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:173
double ymax() const
Returns maximum y-coordinate.
Definition: Boundary.cpp:143
bool intersects(const Position &p1, const Position &p2) const
Returns the information whether this list of points interesects the given line.