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-2015 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 <utils/geom/Line.h>
41 #include "Boundary.h"
42 #include "GeomHelper.h"
43 
44 #ifdef CHECK_MEMORY_LEAKS
45 #include <foreign/nvwa/debug_new.h>
46 #endif // CHECK_MEMORY_LEAKS
47 
48 // ===========================================================================
49 // static members
50 // ===========================================================================
52 
53 
54 // ===========================================================================
55 // method definitions
56 // ===========================================================================
57 bool
59  const SUMOReal x2, const SUMOReal y2,
60  const SUMOReal x3, const SUMOReal y3,
61  const SUMOReal x4, const SUMOReal y4,
62  SUMOReal* x, SUMOReal* y, SUMOReal* mu) {
63  const SUMOReal eps = std::numeric_limits<SUMOReal>::epsilon();
64  const double denominator = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1);
65  const double numera = (x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3);
66  const double numerb = (x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3);
67  /* Are the lines coincident? */
68  if (fabs(numera) < eps && fabs(numerb) < eps && fabs(denominator) < eps) {
69  SUMOReal a1;
70  SUMOReal a2;
71  SUMOReal a3;
72  SUMOReal a4;
73  SUMOReal a = -1e12;
74  if (x1 != x2) {
75  a1 = x1 < x2 ? x1 : x2;
76  a2 = x1 < x2 ? x2 : x1;
77  a3 = x3 < x4 ? x3 : x4;
78  a4 = x3 < x4 ? x4 : x3;
79  } else {
80  a1 = y1 < y2 ? y1 : y2;
81  a2 = y1 < y2 ? y2 : y1;
82  a3 = y3 < y4 ? y3 : y4;
83  a4 = y3 < y4 ? y4 : y3;
84  }
85  if (a1 <= a3 && a3 <= a2) {
86  if (a4 < a2) {
87  a = (a3 + a4) / 2;
88  } else {
89  a = (a2 + a3) / 2;
90  }
91  }
92  if (a3 <= a1 && a1 <= a4) {
93  if (a2 < a4) {
94  a = (a1 + a2) / 2;
95  } else {
96  a = (a1 + a4) / 2;
97  }
98  }
99  if (a != -1e12) {
100  if (x != 0) {
101  if (x1 != x2) {
102  *mu = (a - x1) / (x2 - x1);
103  *x = a;
104  *y = y1 + (*mu) * (y2 - y1);
105  } else {
106  *x = x1;
107  *y = a;
108  if (y2 == y1) {
109  *mu = 0;
110  } else {
111  *mu = (a - y1) / (y2 - y1);
112  }
113  }
114  }
115  return true;
116  }
117  return false;
118  }
119  /* Are the lines parallel */
120  if (fabs(denominator) < eps) {
121  return false;
122  }
123  /* Is the intersection along the segments */
124  double mua = numera / denominator;
125  /* reduce rounding errors for lines ending in the same point */
126  if (fabs(x2 - x4) < eps && fabs(y2 - y4) < eps) {
127  mua = 1.;
128  } else {
129  const double mub = numerb / denominator;
130  if (mua < 0 || mua > 1 || mub < 0 || mub > 1) {
131  return false;
132  }
133  }
134  if (x != 0) {
135  *x = x1 + mua * (x2 - x1);
136  *y = y1 + mua * (y2 - y1);
137  *mu = mua;
138  }
139  return true;
140 }
141 
142 
143 
144 bool
145 GeomHelper::intersects(const Position& p11, const Position& p12,
146  const Position& p21, const Position& p22) {
147  return intersects(p11.x(), p11.y(), p12.x(), p12.y(),
148  p21.x(), p21.y(), p22.x(), p22.y(), 0, 0, 0);
149 }
150 
151 
152 bool
153 GeomHelper::pointOnLine(const Position& p, const Position& from, const Position& to) {
154  if (p.x() >= MIN2(from.x(), to.x()) && p.x() <= MAX2(from.x(), to.x()) &&
155  p.y() >= MIN2(from.y(), to.y()) && p.y() <= MAX2(from.y(), to.y())) {
156  return true;
157  }
158  return false;
159 }
160 
161 
162 void
164  std::vector<SUMOReal>& into) {
165  SUMOReal dx = p2.x() - p1.x();
166  SUMOReal dy = p2.y() - p1.y();
167 
168  SUMOReal A = dx * dx + dy * dy;
169  SUMOReal B = 2 * (dx * (p1.x() - c.x()) + dy * (p1.y() - c.y()));
170  SUMOReal C = (p1.x() - c.x()) * (p1.x() - c.x()) + (p1.y() - c.y()) * (p1.y() - c.y()) - radius * radius;
171 
172  SUMOReal det = B * B - 4 * A * C;
173  if ((A <= 0.0000001) || (det < 0)) {
174  // No real solutions.
175  return;
176  } else if (det == 0) {
177  // One solution.
178  SUMOReal t = -B / (2 * A);
179  Position intersection(p1.x() + t * dx, p1.y() + t * dy);
180  if (GeomHelper::pointOnLine(intersection, p1, p2)) {
181  into.push_back(t);
182  }
183  return;
184  } else {
185  // Two solutions.
186  SUMOReal t = (float)((-B + sqrt(det)) / (2 * A));
187  Position intersection(p1.x() + t * dx, p1.y() + t * dy);
188  if (GeomHelper::pointOnLine(intersection, p1, p2)) {
189  into.push_back(t);
190  }
191  t = (float)((-B - sqrt(det)) / (2 * A));
192  intersection.set(p1.x() + t * dx, p1.y() + t * dy);
193  if (GeomHelper::pointOnLine(intersection, p1, p2)) {
194  into.push_back(t);
195  }
196  return;
197  }
198 }
199 
200 
201 
202 Position
204  const Position& p12,
205  const Position& p21,
206  const Position& p22) {
207  SUMOReal x, y, m;
208  if (intersects(p11.x(), p11.y(), p12.x(), p12.y(),
209  p21.x(), p21.y(), p22.x(), p22.y(), &x, &y, &m)) {
210  // @todo calculate better "average" z value
211  return Position(x, y, p11.z() + m * (p12.z() - p11.z()));
212  }
213  return Position::INVALID;
214 }
215 
216 
217 
218 /*
219  Return the angle between two vectors on a plane
220  The angle is from vector 1 to vector 2, positive anticlockwise
221  The result is between -pi -> pi
222 */
223 SUMOReal
225  SUMOReal dtheta = atan2(y2, x2) - atan2(y1, x1);
226  while (dtheta > (SUMOReal) M_PI) {
227  dtheta -= (SUMOReal)(2.0 * M_PI);
228  }
229  while (dtheta < (SUMOReal) - M_PI) {
230  dtheta += (SUMOReal)(2.0 * M_PI);
231  }
232  return dtheta;
233 }
234 
235 
236 Position
238  const Position& p2, SUMOReal length) {
239  const SUMOReal factor = length / p1.distanceTo(p2);
240  return p1 + (p2 - p1) * factor;
241 }
242 
243 
244 Position
246  const Position& p2, SUMOReal length) {
247  const SUMOReal factor = length / p1.distanceTo(p2);
248  return p1 - (p2 - p1) * factor;
249 }
250 
251 
252 Position
254  const Position& p2, SUMOReal length) {
255  const SUMOReal factor = length / p1.distanceTo(p2);
256  return p2 - (p1 - p2) * factor;
257 }
258 
259 
260 SUMOReal
262  const Position& LineEnd,
263  const Position& Point, bool perpendicular) {
264  const SUMOReal lineLength2D = LineStart.distanceTo2D(LineEnd);
265  if (lineLength2D == 0.0f) {
266  return 0.0f;
267  } else {
268  // scalar product equals length of orthogonal projection times length of vector being projected onto
269  // dividing the scalar product by the square of the distance gives the relative position
270  const SUMOReal u = (((Point.x() - LineStart.x()) * (LineEnd.x() - LineStart.x())) +
271  ((Point.y() - LineStart.y()) * (LineEnd.y() - LineStart.y()))
272  ) / (lineLength2D * lineLength2D);
273  if (u < 0.0f || u > 1.0f) { // closest point does not fall within the line segment
274  if (perpendicular) {
275  return -1;
276  }
277  if (u < 0.0f) {
278  return 0.0f;
279  }
280  return lineLength2D;
281  }
282  return u * lineLength2D;
283  }
284 }
285 
286 
287 SUMOReal
289  const Position& lineStart,
290  const Position& lineEnd) {
291  const SUMOReal lineLengthSquared = lineStart.distanceSquaredTo(lineEnd);
292  if (lineLengthSquared == 0.0f) {
293  return point.distanceTo(lineStart);
294  } else {
295  // scalar product equals length of orthogonal projection times length of vector being projected onto
296  // dividing the scalar product by the square of the distance gives the relative position
297  SUMOReal u = (((point.x() - lineStart.x()) * (lineEnd.x() - lineStart.x())) +
298  ((point.y() - lineStart.y()) * (lineEnd.y() - lineStart.y()))
299  ) / lineLengthSquared;
300  if (u < 0.0f || u > 1.0f) {
301  return -1; // closest point does not fall within the line segment
302  }
303  Position intersection(
304  lineStart.x() + u * (lineEnd.x() - lineStart.x()),
305  lineStart.y() + u * (lineEnd.y() - lineStart.y()));
306  return point.distanceTo(intersection);
307  }
308 }
309 
310 
311 SUMOReal
313  const Position& lineStart,
314  const Position& lineEnd,
315  Position& outIntersection) {
316  const SUMOReal length = nearest_offset_on_line_to_point2D(lineStart, lineEnd, point, false);
317  outIntersection.set(Line(lineStart, lineEnd).getPositionAtDistance2D(length));
318  return point.distanceTo2D(outIntersection);
319 }
320 
321 
322 
323 Position
325  const Position& lineBeg,
326  const Position& lineEnd,
327  SUMOReal amount) {
328  const SUMOReal dx = lineBeg.x() - lineEnd.x();
329  const SUMOReal dy = lineBeg.y() - lineEnd.y();
330  const SUMOReal length = sqrt(dx * dx + dy * dy);
331  if (length > 0) {
332  p.add(dy * amount / length, -dx * amount / length);
333  }
334  return p;
335 }
336 
337 
338 
339 Position
341  if (v.intersects(Position(b.xmin(), b.ymin()), Position(b.xmin(), b.ymax()))) {
342  return v.intersectsAtPoint(
343  Position(b.xmin(), b.ymin()),
344  Position(b.xmin(), b.ymax()));
345  }
346  if (v.intersects(Position(b.xmax(), b.ymin()), Position(b.xmax(), b.ymax()))) {
347  return v.intersectsAtPoint(
348  Position(b.xmax(), b.ymin()),
349  Position(b.xmax(), b.ymax()));
350  }
351  if (v.intersects(Position(b.xmin(), b.ymin()), Position(b.xmax(), b.ymin()))) {
352  return v.intersectsAtPoint(
353  Position(b.xmin(), b.ymin()),
354  Position(b.xmax(), b.ymin()));
355  }
356  if (v.intersects(Position(b.xmin(), b.ymax()), Position(b.xmax(), b.ymax()))) {
357  return v.intersectsAtPoint(
358  Position(b.xmin(), b.ymax()),
359  Position(b.xmax(), b.ymax()));
360  }
361  throw 1;
362 }
363 
364 std::pair<SUMOReal, SUMOReal>
366  const Position& end,
367  SUMOReal wanted_offset) {
368  return getNormal90D_CW(beg, end, beg.distanceTo2D(end), wanted_offset);
369 }
370 
371 
372 std::pair<SUMOReal, SUMOReal>
374  const Position& end,
375  SUMOReal length, SUMOReal wanted_offset) {
376  if (beg == end) {
377  throw InvalidArgument("same points at " + toString(beg));
378  }
379  return std::pair<SUMOReal, SUMOReal>
380  ((beg.y() - end.y()) * wanted_offset / length, (end.x() - beg.x()) * wanted_offset / length);
381 }
382 
383 
384 SUMOReal
386  SUMOReal v = angle2 - angle1;
387  if (v < 0) {
388  v = 360 + v;
389  }
390  return v;
391 }
392 
393 
394 SUMOReal
396  SUMOReal v = angle1 - angle2;
397  if (v < 0) {
398  v = 360 + v;
399  }
400  return v;
401 }
402 
403 
404 SUMOReal
406  return MIN2(getCWAngleDiff(angle1, angle2), getCCWAngleDiff(angle1, angle2));
407 }
408 
409 
410 SUMOReal
412  return MAX2(getCWAngleDiff(angle1, angle2), getCCWAngleDiff(angle1, angle2));
413 }
414 
415 
416 
417 /****************************************************************************/
418 
static std::pair< SUMOReal, SUMOReal > getNormal90D_CW(const Position &beg, const Position &end, SUMOReal length, SUMOReal wanted_offset)
Definition: GeomHelper.cpp:373
static SUMOReal Angle2D(SUMOReal x1, SUMOReal y1, SUMOReal x2, SUMOReal y2)
Definition: GeomHelper.cpp:224
static SUMOReal getCWAngleDiff(SUMOReal angle1, SUMOReal angle2)
Returns the distance of second angle from first angle clockwise.
Definition: GeomHelper.cpp:395
static SUMOReal getCCWAngleDiff(SUMOReal angle1, SUMOReal angle2)
Returns the distance of second angle from first angle counter-clockwise.
Definition: GeomHelper.cpp:385
static Position intersection_position2D(const Position &p11, const Position &p12, const Position &p21, const Position &p22)
returns the intersection point of the (infinite) lines p11,p12 and p21,p22. If the given lines are pa...
Definition: GeomHelper.cpp:203
static Position interpolate(const Position &p1, const Position &p2, SUMOReal length)
Definition: GeomHelper.cpp:237
void add(const Position &pos)
Adds the given position to this one.
Definition: Position.h:119
SUMOReal distanceSquaredTo(const Position &p2) const
Definition: Position.h:234
#define M_PI
Definition: angles.h:37
SUMOReal ymin() const
Returns minimum y-coordinate.
Definition: Boundary.cpp:124
SUMOReal xmin() const
Returns minimum x-coordinate.
Definition: Boundary.cpp:112
bool intersects(const Position &p1, const Position &p2) const
T MAX2(T a, T b)
Definition: StdDefs.h:74
static Position extrapolate_second(const Position &p1, const Position &p2, SUMOReal length)
Definition: GeomHelper.cpp:253
SUMOReal distanceTo(const Position &p2) const
returns the euclidean distance in 3 dimension
Definition: Position.h:229
static Position extrapolate_first(const Position &p1, const Position &p2, SUMOReal length)
Definition: GeomHelper.cpp:245
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:163
static Position crossPoint(const Boundary &b, const PositionVector &v)
Definition: GeomHelper.cpp:340
SUMOReal x() const
Returns the x-position.
Definition: Position.h:63
SUMOReal xmax() const
Returns maximum x-coordinate.
Definition: Boundary.cpp:118
A class that stores a 2D geometrical boundary.
Definition: Boundary.h:48
static SUMOReal nearest_offset_on_line_to_point2D(const Position &l1, const Position &l2, const Position &p, bool perpendicular=true)
Definition: GeomHelper.cpp:261
static bool intersects(const Position &p11, const Position &p12, const Position &p21, const Position &p22)
return whether given lines intersect
Definition: GeomHelper.cpp:145
A point in 2D or 3D with translation and scaling methods.
Definition: Position.h:46
static Position transfer_to_side(Position &p, const Position &lineBeg, const Position &lineEnd, SUMOReal amount)
Definition: GeomHelper.cpp:324
A list of positions.
static SUMOReal distancePointLine(const Position &point, const Position &lineStart, const Position &lineEnd)
Definition: GeomHelper.cpp:288
SUMOReal z() const
Returns the z-position.
Definition: Position.h:73
static SUMOReal closestDistancePointLine2D(const Position &point, const Position &lineStart, const Position &lineEnd, Position &outIntersection)
Definition: GeomHelper.cpp:312
Definition: Line.h:51
T MIN2(T a, T b)
Definition: StdDefs.h:68
std::string toString(const T &t, std::streamsize accuracy=OUTPUT_ACCURACY)
Definition: ToString.h:53
Position intersectsAtPoint(const Position &p1, const Position &p2) const
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:405
void set(SUMOReal x, SUMOReal y)
Definition: Position.h:78
static SUMOReal getMaxAngleDiff(SUMOReal angle1, SUMOReal angle2)
Returns the maximum distance (clockwise/counter-clockwise) between both angles.
Definition: GeomHelper.cpp:411
SUMOReal distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
Definition: Position.h:240
#define SUMOReal
Definition: config.h:218
SUMOReal ymax() const
Returns maximum y-coordinate.
Definition: Boundary.cpp:130
static bool pointOnLine(const Position &p, const Position &from, const Position &to)
Returns whether the given point lies on the given line.
Definition: GeomHelper.cpp:153
static const SUMOReal INVALID_OFFSET
a value to signify offsets outside the range of [0, Line.length()]
Definition: GeomHelper.h:59
static const Position INVALID
Definition: Position.h:262