SUMO - Simulation of Urban MObility
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
GeomHelper.cpp
Go to the documentation of this file.
1 /****************************************************************************/
10 // Some geometrical helpers
11 /****************************************************************************/
12 // SUMO, Simulation of Urban MObility; see http://sumo-sim.org/
13 // Copyright (C) 2001-2013 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 // ===========================================================================
50 // method definitions
51 // ===========================================================================
52 bool
54  const SUMOReal x2, const SUMOReal y2,
55  const SUMOReal x3, const SUMOReal y3,
56  const SUMOReal x4, const SUMOReal y4,
57  SUMOReal* x, SUMOReal* y, SUMOReal* mu) {
58  const SUMOReal eps = std::numeric_limits<SUMOReal>::epsilon();
59  const double denominator = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1);
60  const double numera = (x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3);
61  const double numerb = (x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3);
62  /* Are the lines coincident? */
63  if (fabs(numera) < eps && fabs(numerb) < eps && fabs(denominator) < eps) {
64  SUMOReal a1;
65  SUMOReal a2;
66  SUMOReal a3;
67  SUMOReal a4;
68  SUMOReal a = -1e12;
69  if (x1 != x2) {
70  a1 = x1 < x2 ? x1 : x2;
71  a2 = x1 < x2 ? x2 : x1;
72  a3 = x3 < x4 ? x3 : x4;
73  a4 = x3 < x4 ? x4 : x3;
74  } else {
75  a1 = y1 < y2 ? y1 : y2;
76  a2 = y1 < y2 ? y2 : y1;
77  a3 = y3 < y4 ? y3 : y4;
78  a4 = y3 < y4 ? y4 : y3;
79  }
80  if (a1 <= a3 && a3 <= a2) {
81  if (a4 < a2) {
82  a = (a3 + a4) / 2;
83  } else {
84  a = (a2 + a3) / 2;
85  }
86  }
87  if (a3 <= a1 && a1 <= a4) {
88  if (a2 < a4) {
89  a = (a1 + a2) / 2;
90  } else {
91  a = (a1 + a4) / 2;
92  }
93  }
94  if (a != -1e12) {
95  if (x != 0) {
96  if (x1 != x2) {
97  *mu = (a - x1) / (x2 - x1);
98  *x = a;
99  *y = y1 + (*mu) * (y2 - y1);
100  } else {
101  *x = x1;
102  *y = a;
103  *mu = (a - y1) / (y2 - y1);
104  }
105  }
106  return true;
107  }
108  return false;
109  }
110  /* Are the lines parallel */
111  if (fabs(denominator) < eps) {
112  return false;
113  }
114  /* Is the intersection along the segments */
115  const double mua = numera / denominator;
116  const double mub = numerb / denominator;
117  if (mua < 0 || mua > 1 || mub < 0 || mub > 1) {
118  return false;
119  }
120  if (x != 0) {
121  *x = x1 + mua * (x2 - x1);
122  *y = y1 + mua * (y2 - y1);
123  *mu = mua;
124  }
125  return true;
126 }
127 
128 
129 
130 
131 bool
132 GeomHelper::intersects(const Position& p11, const Position& p12,
133  const Position& p21, const Position& p22) {
134  return intersects(p11.x(), p11.y(), p12.x(), p12.y(),
135  p21.x(), p21.y(), p22.x(), p22.y(), 0, 0, 0);
136 }
137 
138 
139 Position
141  const Position& p12,
142  const Position& p21,
143  const Position& p22) {
144  SUMOReal x, y, m;
145  if (intersects(p11.x(), p11.y(), p12.x(), p12.y(),
146  p21.x(), p21.y(), p22.x(), p22.y(), &x, &y, &m)) {
147  // @todo calculate better "average" z value
148  return Position(x, y, p11.z() + m * (p12.z() - p11.z()));
149  }
150  return Position(-1, -1);
151 }
152 
153 
154 
155 /*
156  Return the angle between two vectors on a plane
157  The angle is from vector 1 to vector 2, positive anticlockwise
158  The result is between -pi -> pi
159 */
160 SUMOReal
162  SUMOReal dtheta = atan2(y2, x2) - atan2(y1, x1);
163  while (dtheta > (SUMOReal) M_PI) {
164  dtheta -= (SUMOReal)(2.0 * M_PI);
165  }
166  while (dtheta < (SUMOReal) - M_PI) {
167  dtheta += (SUMOReal)(2.0 * M_PI);
168  }
169  return dtheta;
170 }
171 
172 
173 Position
175  const Position& p2, SUMOReal length) {
176  const SUMOReal factor = length / p1.distanceTo(p2);
177  return p1 + (p2 - p1) * factor;
178 }
179 
180 
181 Position
183  const Position& p2, SUMOReal length) {
184  const SUMOReal factor = length / p1.distanceTo(p2);
185  return p1 - (p2 - p1) * factor;
186 }
187 
188 
189 Position
191  const Position& p2, SUMOReal length) {
192  const SUMOReal factor = length / p1.distanceTo(p2);
193  return p2 - (p1 - p2) * factor;
194 }
195 
196 
197 SUMOReal
199  const Position& LineEnd,
200  const Position& Point, bool perpendicular) {
201  const SUMOReal lineLength2D = LineStart.distanceTo2D(LineEnd);
202  if (lineLength2D == 0.0f) {
203  return 0.0f;
204  } else {
205  // scalar product equals length of orthogonal projection times length of vector being projected onto
206  // dividing the scalar product by the square of the distance gives the relative position
207  const SUMOReal u = (((Point.x() - LineStart.x()) * (LineEnd.x() - LineStart.x())) +
208  ((Point.y() - LineStart.y()) * (LineEnd.y() - LineStart.y()))
209  ) / (lineLength2D * lineLength2D);
210  if (u < 0.0f || u > 1.0f) { // closest point does not fall within the line segment
211  if (perpendicular) {
212  return -1;
213  }
214  if (u < 0.0f) {
215  return 0.0f;
216  }
217  return lineLength2D;
218  }
219  return u * lineLength2D;
220  }
221 }
222 
223 
224 SUMOReal
226  const Position& lineStart,
227  const Position& lineEnd) {
228  const SUMOReal lineLengthSquared = lineStart.distanceSquaredTo(lineEnd);
229  if (lineLengthSquared == 0.0f) {
230  return point.distanceTo(lineStart);
231  } else {
232  // scalar product equals length of orthogonal projection times length of vector being projected onto
233  // dividing the scalar product by the square of the distance gives the relative position
234  SUMOReal u = (((point.x() - lineStart.x()) * (lineEnd.x() - lineStart.x())) +
235  ((point.y() - lineStart.y()) * (lineEnd.y() - lineStart.y()))
236  ) / lineLengthSquared;
237  if (u < 0.0f || u > 1.0f) {
238  return -1; // closest point does not fall within the line segment
239  }
240  Position intersection(
241  lineStart.x() + u * (lineEnd.x() - lineStart.x()),
242  lineStart.y() + u * (lineEnd.y() - lineStart.y()));
243  return point.distanceTo(intersection);
244  }
245 }
246 
247 
248 SUMOReal
250  const Position& lineStart,
251  const Position& lineEnd,
252  Position& outIntersection) {
253  const SUMOReal length = nearest_offset_on_line_to_point2D(lineStart, lineEnd, point, false);
254  outIntersection.set(Line(lineStart, lineEnd).getPositionAtDistance(length));
255  return point.distanceTo2D(outIntersection);
256 }
257 
258 
259 
260 Position
262  const Position& lineBeg,
263  const Position& lineEnd,
264  SUMOReal amount) {
265  const SUMOReal dx = lineBeg.x() - lineEnd.x();
266  const SUMOReal dy = lineBeg.y() - lineEnd.y();
267  const SUMOReal length = sqrt(dx * dx + dy * dy);
268  if (length > 0) {
269  p.add(dy * amount / length, -dx * amount / length);
270  }
271  return p;
272 }
273 
274 
275 
276 Position
278  if (v.intersects(Position(b.xmin(), b.ymin()), Position(b.xmin(), b.ymax()))) {
279  return v.intersectsAtPoint(
280  Position(b.xmin(), b.ymin()),
281  Position(b.xmin(), b.ymax()));
282  }
283  if (v.intersects(Position(b.xmax(), b.ymin()), Position(b.xmax(), b.ymax()))) {
284  return v.intersectsAtPoint(
285  Position(b.xmax(), b.ymin()),
286  Position(b.xmax(), b.ymax()));
287  }
288  if (v.intersects(Position(b.xmin(), b.ymin()), Position(b.xmax(), b.ymin()))) {
289  return v.intersectsAtPoint(
290  Position(b.xmin(), b.ymin()),
291  Position(b.xmax(), b.ymin()));
292  }
293  if (v.intersects(Position(b.xmin(), b.ymax()), Position(b.xmax(), b.ymax()))) {
294  return v.intersectsAtPoint(
295  Position(b.xmin(), b.ymax()),
296  Position(b.xmax(), b.ymax()));
297  }
298  throw 1;
299 }
300 
301 std::pair<SUMOReal, SUMOReal>
303  const Position& end,
304  SUMOReal wanted_offset) {
305  return getNormal90D_CW(beg, end, beg.distanceTo2D(end), wanted_offset);
306 }
307 
308 
309 std::pair<SUMOReal, SUMOReal>
311  const Position& end,
312  SUMOReal length, SUMOReal wanted_offset) {
313  if (beg == end) {
314  throw InvalidArgument("same points at " + toString(beg));
315  }
316  return std::pair<SUMOReal, SUMOReal>
317  ((beg.y() - end.y()) * wanted_offset / length, (end.x() - beg.x()) * wanted_offset / length);
318 }
319 
320 
321 SUMOReal
323  SUMOReal v = angle2 - angle1;
324  if (v < 0) {
325  v = 360 + v;
326  }
327  return v;
328 }
329 
330 
331 SUMOReal
333  SUMOReal v = angle1 - angle2;
334  if (v < 0) {
335  v = 360 + v;
336  }
337  return v;
338 }
339 
340 
341 SUMOReal
343  return MIN2(getCWAngleDiff(angle1, angle2), getCCWAngleDiff(angle1, angle2));
344 }
345 
346 
347 SUMOReal
349  return MAX2(getCWAngleDiff(angle1, angle2), getCCWAngleDiff(angle1, angle2));
350 }
351 
352 
353 
354 /****************************************************************************/
355 
static std::pair< SUMOReal, SUMOReal > getNormal90D_CW(const Position &beg, const Position &end, SUMOReal length, SUMOReal wanted_offset)
Definition: GeomHelper.cpp:310
static SUMOReal Angle2D(SUMOReal x1, SUMOReal y1, SUMOReal x2, SUMOReal y2)
Definition: GeomHelper.cpp:161
static SUMOReal getCWAngleDiff(SUMOReal angle1, SUMOReal angle2)
Returns the distance of second angle from first angle clockwise.
Definition: GeomHelper.cpp:332
static SUMOReal getCCWAngleDiff(SUMOReal angle1, SUMOReal angle2)
Returns the distance of second angle from first angle counter-clockwise.
Definition: GeomHelper.cpp:322
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:140
static Position interpolate(const Position &p1, const Position &p2, SUMOReal length)
Definition: GeomHelper.cpp:174
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:213
#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:63
static Position extrapolate_second(const Position &p1, const Position &p2, SUMOReal length)
Definition: GeomHelper.cpp:190
SUMOReal distanceTo(const Position &p2) const
returns the euclidean distance in 3 dimension
Definition: Position.h:208
static Position extrapolate_first(const Position &p1, const Position &p2, SUMOReal length)
Definition: GeomHelper.cpp:182
static Position crossPoint(const Boundary &b, const PositionVector &v)
Definition: GeomHelper.cpp:277
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:198
static bool intersects(const Position &p11, const Position &p12, const Position &p21, const Position &p22)
return whether the line segments defined by Line p11,p12 and Line p21,p22 intersect ...
Definition: GeomHelper.cpp:132
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:261
A list of positions.
static SUMOReal distancePointLine(const Position &point, const Position &lineStart, const Position &lineEnd)
Definition: GeomHelper.cpp:225
SUMOReal z() const
Returns the z-position.
Definition: Position.h:73
Definition: Line.h:51
T MIN2(T a, T b)
Definition: StdDefs.h:57
std::string toString(const T &t, std::streamsize accuracy=OUTPUT_ACCURACY)
Definition: ToString.h:51
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:342
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:348
SUMOReal distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
Definition: Position.h:219
#define SUMOReal
Definition: config.h:221
SUMOReal ymax() const
Returns maximum y-coordinate.
Definition: Boundary.cpp:130
static SUMOReal closestDistancePointLine(const Position &point, const Position &lineStart, const Position &lineEnd, Position &outIntersection)
Definition: GeomHelper.cpp:249