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