SUMO - Simulation of Urban MObility
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Modules Pages
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-2016 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 #ifdef CHECK_MEMORY_LEAKS
44 #include <foreign/nvwa/debug_new.h>
45 #endif // CHECK_MEMORY_LEAKS
46 
47 // ===========================================================================
48 // static members
49 // ===========================================================================
51 
52 
53 // ===========================================================================
54 // method definitions
55 // ===========================================================================
56 
57 void
59  std::vector<SUMOReal>& into) {
60  const SUMOReal dx = p2.x() - p1.x();
61  const SUMOReal dy = p2.y() - p1.y();
62 
63  const SUMOReal A = dx * dx + dy * dy;
64  const SUMOReal B = 2 * (dx * (p1.x() - c.x()) + dy * (p1.y() - c.y()));
65  const SUMOReal C = (p1.x() - c.x()) * (p1.x() - c.x()) + (p1.y() - c.y()) * (p1.y() - c.y()) - radius * radius;
66 
67  const SUMOReal det = B * B - 4 * A * C;
68  if ((A <= 0.0000001) || (det < 0)) {
69  // No real solutions.
70  return;
71  }
72  if (det == 0) {
73  // One solution.
74  const SUMOReal t = -B / (2 * A);
75  if (t >= 0. && t <= 1.) {
76  into.push_back(t);
77  }
78  } else {
79  // Two solutions.
80  const SUMOReal t = (SUMOReal)((-B + sqrt(det)) / (2 * A));
81  Position intersection(p1.x() + t * dx, p1.y() + t * dy);
82  if (t >= 0. && t <= 1.) {
83  into.push_back(t);
84  }
85  const SUMOReal t2 = (SUMOReal)((-B - sqrt(det)) / (2 * A));
86  if (t2 >= 0. && t2 <= 1.) {
87  into.push_back(t2);
88  }
89  }
90 }
91 
92 
94 GeomHelper::angle2D(const Position& p1, const Position& p2) {
95  return angleDiff(atan2(p1.y(), p1.x()), atan2(p2.y(), p2.x()));
96 }
97 
98 
101  const Position& lineEnd,
102  const Position& p, bool perpendicular) {
103  const SUMOReal lineLength2D = lineStart.distanceTo2D(lineEnd);
104  if (lineLength2D == 0.0f) {
105  return 0.0f;
106  } else {
107  // scalar product equals length of orthogonal projection times length of vector being projected onto
108  // dividing the scalar product by the square of the distance gives the relative position
109  const SUMOReal u = (((p.x() - lineStart.x()) * (lineEnd.x() - lineStart.x())) +
110  ((p.y() - lineStart.y()) * (lineEnd.y() - lineStart.y()))
111  ) / (lineLength2D * lineLength2D);
112  if (u < 0.0f || u > 1.0f) { // closest point does not fall within the line segment
113  if (perpendicular) {
114  return INVALID_OFFSET;
115  }
116  if (u < 0.0f) {
117  return 0.0f;
118  }
119  return lineLength2D;
120  }
121  return u * lineLength2D;
122  }
123 }
124 
125 
126 Position
128  if (v.intersects(Position(b.xmin(), b.ymin()), Position(b.xmin(), b.ymax()))) {
129  return v.intersectionPosition2D(
130  Position(b.xmin(), b.ymin()),
131  Position(b.xmin(), b.ymax()));
132  }
133  if (v.intersects(Position(b.xmax(), b.ymin()), Position(b.xmax(), b.ymax()))) {
134  return v.intersectionPosition2D(
135  Position(b.xmax(), b.ymin()),
136  Position(b.xmax(), b.ymax()));
137  }
138  if (v.intersects(Position(b.xmin(), b.ymin()), Position(b.xmax(), b.ymin()))) {
139  return v.intersectionPosition2D(
140  Position(b.xmin(), b.ymin()),
141  Position(b.xmax(), b.ymin()));
142  }
143  if (v.intersects(Position(b.xmin(), b.ymax()), Position(b.xmax(), b.ymax()))) {
144  return v.intersectionPosition2D(
145  Position(b.xmin(), b.ymax()),
146  Position(b.xmax(), b.ymax()));
147  }
148  throw 1;
149 }
150 
151 SUMOReal
153  SUMOReal v = angle2 - angle1;
154  if (v < 0) {
155  v = 360 + v;
156  }
157  return v;
158 }
159 
160 
161 SUMOReal
163  SUMOReal v = angle1 - angle2;
164  if (v < 0) {
165  v = 360 + v;
166  }
167  return v;
168 }
169 
170 
171 SUMOReal
173  return MIN2(getCWAngleDiff(angle1, angle2), getCCWAngleDiff(angle1, angle2));
174 }
175 
176 
177 SUMOReal
178 GeomHelper::angleDiff(const SUMOReal angle1, const SUMOReal angle2) {
179  SUMOReal dtheta = angle2 - angle1;
180  while (dtheta > (SUMOReal) M_PI) {
181  dtheta -= (SUMOReal)(2.0 * M_PI);
182  }
183  while (dtheta < (SUMOReal) - M_PI) {
184  dtheta += (SUMOReal)(2.0 * M_PI);
185  }
186  return dtheta;
187 }
188 
189 
190 SUMOReal
192  SUMOReal degree = RAD2DEG(M_PI / 2. - angle);
193  while (degree >= 360.) {
194  degree -= 360.;
195  }
196  while (degree < 0.) {
197  degree += 360.;
198  }
199  return degree;
200 }
201 
202 
203 SUMOReal
204 GeomHelper::legacyDegree(const SUMOReal angle, const bool positive) {
205  SUMOReal degree = -RAD2DEG(M_PI / 2. + angle);
206  if (positive) {
207  while (degree >= 360.) {
208  degree -= 360.;
209  }
210  while (degree < 0.) {
211  degree += 360.;
212  }
213  } else {
214  while (degree >= 180.) {
215  degree -= 360.;
216  }
217  while (degree < -180.) {
218  degree += 360.;
219  }
220  }
221  return degree;
222 }
223 
224 
225 /****************************************************************************/
226 
static SUMOReal 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:94
static SUMOReal getCWAngleDiff(SUMOReal angle1, SUMOReal angle2)
Returns the distance of second angle from first angle clockwise.
Definition: GeomHelper.cpp:162
static SUMOReal getCCWAngleDiff(SUMOReal angle1, SUMOReal angle2)
Returns the distance of second angle from first angle counter-clockwise.
Definition: GeomHelper.cpp:152
#define M_PI
Definition: angles.h:37
SUMOReal ymin() const
Returns minimum y-coordinate.
Definition: Boundary.cpp:142
SUMOReal xmin() const
Returns minimum x-coordinate.
Definition: Boundary.cpp:130
bool intersects(const Position &p1, const Position &p2) const
Returns the information whether this list of points interesects the given line.
#define RAD2DEG(x)
Definition: GeomHelper.h:46
static Position crossPoint(const Boundary &b, const PositionVector &v)
Definition: GeomHelper.cpp:127
static SUMOReal angleDiff(const SUMOReal angle1, const SUMOReal angle2)
Returns the difference of the second angle to the first angle in radiants.
Definition: GeomHelper.cpp:178
SUMOReal x() const
Returns the x-position.
Definition: Position.h:63
SUMOReal xmax() const
Returns maximum x-coordinate.
Definition: Boundary.cpp:136
A class that stores a 2D geometrical boundary.
Definition: Boundary.h:48
static SUMOReal legacyDegree(const SUMOReal angle, const bool positive=false)
Definition: GeomHelper.cpp:204
A point in 2D or 3D with translation and scaling methods.
Definition: Position.h:46
A list of positions.
static void findLineCircleIntersections(const Position &c, SUMOReal radius, const Position &p1, const Position &p2, std::vector< SUMOReal > &into)
Returns the positions the given circle is crossed by the given line.
Definition: GeomHelper.cpp:58
T MIN2(T a, T b)
Definition: StdDefs.h:69
static SUMOReal naviDegree(const SUMOReal angle)
Definition: GeomHelper.cpp:191
SUMOReal y() const
Returns the y-position.
Definition: Position.h:68
static SUMOReal getMinAngleDiff(SUMOReal angle1, SUMOReal angle2)
Returns the minimum distance (clockwise/counter-clockwise) between both angles.
Definition: GeomHelper.cpp:172
static SUMOReal nearest_offset_on_line_to_point2D(const Position &lineStart, const Position &lineEnd, const Position &p, bool perpendicular=true)
Definition: GeomHelper.cpp:100
Position intersectionPosition2D(const Position &p1, const Position &p2, const SUMOReal withinDist=0.) const
Returns the position of the intersection.
SUMOReal distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
Definition: Position.h:232
#define SUMOReal
Definition: config.h:214
SUMOReal ymax() const
Returns maximum y-coordinate.
Definition: Boundary.cpp:148
static const SUMOReal INVALID_OFFSET
a value to signify offsets outside the range of [0, Line.length()]
Definition: GeomHelper.h:59