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 static methods performing geometrical operations
11 /****************************************************************************/
12 // SUMO, Simulation of Urban MObility; see http://sumo-sim.org/
13 // Copyright (C) 2001-2014 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  if (y2 == y1) {
104  *mu = 0;
105  } else {
106  *mu = (a - y1) / (y2 - y1);
107  }
108  }
109  }
110  return true;
111  }
112  return false;
113  }
114  /* Are the lines parallel */
115  if (fabs(denominator) < eps) {
116  return false;
117  }
118  /* Is the intersection along the segments */
119  const double mua = numera / denominator;
120  const double mub = numerb / denominator;
121  if (mua < 0 || mua > 1 || mub < 0 || mub > 1) {
122  return false;
123  }
124  if (x != 0) {
125  *x = x1 + mua * (x2 - x1);
126  *y = y1 + mua * (y2 - y1);
127  *mu = mua;
128  }
129  return true;
130 }
131 
132 
133 
134 bool
135 GeomHelper::intersects(const Position& p11, const Position& p12,
136  const Position& p21, const Position& p22) {
137  return intersects(p11.x(), p11.y(), p12.x(), p12.y(),
138  p21.x(), p21.y(), p22.x(), p22.y(), 0, 0, 0);
139 }
140 
141 
142 bool
143 GeomHelper::pointOnLine(const Position& p, const Position& from, const Position& to) {
144  if (p.x() >= MIN2(from.x(), to.x()) && p.x() <= MAX2(from.x(), to.x()) &&
145  p.y() >= MIN2(from.y(), to.y()) && p.y() <= MAX2(from.y(), to.y())) {
146  return true;
147  }
148  return false;
149 }
150 
151 
152 void
154  std::vector<SUMOReal>& into) {
155  SUMOReal dx = p2.x() - p1.x();
156  SUMOReal dy = p2.y() - p1.y();
157 
158  SUMOReal A = dx * dx + dy * dy;
159  SUMOReal B = 2 * (dx * (p1.x() - c.x()) + dy * (p1.y() - c.y()));
160  SUMOReal C = (p1.x() - c.x()) * (p1.x() - c.x()) + (p1.y() - c.y()) * (p1.y() - c.y()) - radius * radius;
161 
162  SUMOReal det = B * B - 4 * A * C;
163  if ((A <= 0.0000001) || (det < 0)) {
164  // No real solutions.
165  return;
166  } else if (det == 0) {
167  // One solution.
168  SUMOReal t = -B / (2 * A);
169  Position intersection(p1.x() + t * dx, p1.y() + t * dy);
170  if (GeomHelper::pointOnLine(intersection, p1, p2)) {
171  into.push_back(t);
172  }
173  return;
174  } else {
175  // Two solutions.
176  SUMOReal t = (float)((-B + sqrt(det)) / (2 * A));
177  Position intersection(p1.x() + t * dx, p1.y() + t * dy);
178  if (GeomHelper::pointOnLine(intersection, p1, p2)) {
179  into.push_back(t);
180  }
181  t = (float)((-B - sqrt(det)) / (2 * A));
182  intersection.set(p1.x() + t * dx, p1.y() + t * dy);
183  if (GeomHelper::pointOnLine(intersection, p1, p2)) {
184  into.push_back(t);
185  }
186  return;
187  }
188 }
189 
190 
191 
192 Position
194  const Position& p12,
195  const Position& p21,
196  const Position& p22) {
197  SUMOReal x, y, m;
198  if (intersects(p11.x(), p11.y(), p12.x(), p12.y(),
199  p21.x(), p21.y(), p22.x(), p22.y(), &x, &y, &m)) {
200  // @todo calculate better "average" z value
201  return Position(x, y, p11.z() + m * (p12.z() - p11.z()));
202  }
203  return Position(-1, -1);
204 }
205 
206 
207 
208 /*
209  Return the angle between two vectors on a plane
210  The angle is from vector 1 to vector 2, positive anticlockwise
211  The result is between -pi -> pi
212 */
213 SUMOReal
215  SUMOReal dtheta = atan2(y2, x2) - atan2(y1, x1);
216  while (dtheta > (SUMOReal) M_PI) {
217  dtheta -= (SUMOReal)(2.0 * M_PI);
218  }
219  while (dtheta < (SUMOReal) - M_PI) {
220  dtheta += (SUMOReal)(2.0 * M_PI);
221  }
222  return dtheta;
223 }
224 
225 
226 Position
228  const Position& p2, SUMOReal length) {
229  const SUMOReal factor = length / p1.distanceTo(p2);
230  return p1 + (p2 - p1) * factor;
231 }
232 
233 
234 Position
236  const Position& p2, SUMOReal length) {
237  const SUMOReal factor = length / p1.distanceTo(p2);
238  return p1 - (p2 - p1) * factor;
239 }
240 
241 
242 Position
244  const Position& p2, SUMOReal length) {
245  const SUMOReal factor = length / p1.distanceTo(p2);
246  return p2 - (p1 - p2) * factor;
247 }
248 
249 
250 SUMOReal
252  const Position& LineEnd,
253  const Position& Point, bool perpendicular) {
254  const SUMOReal lineLength2D = LineStart.distanceTo2D(LineEnd);
255  if (lineLength2D == 0.0f) {
256  return 0.0f;
257  } else {
258  // scalar product equals length of orthogonal projection times length of vector being projected onto
259  // dividing the scalar product by the square of the distance gives the relative position
260  const SUMOReal u = (((Point.x() - LineStart.x()) * (LineEnd.x() - LineStart.x())) +
261  ((Point.y() - LineStart.y()) * (LineEnd.y() - LineStart.y()))
262  ) / (lineLength2D * lineLength2D);
263  if (u < 0.0f || u > 1.0f) { // closest point does not fall within the line segment
264  if (perpendicular) {
265  return -1;
266  }
267  if (u < 0.0f) {
268  return 0.0f;
269  }
270  return lineLength2D;
271  }
272  return u * lineLength2D;
273  }
274 }
275 
276 
277 SUMOReal
279  const Position& lineStart,
280  const Position& lineEnd) {
281  const SUMOReal lineLengthSquared = lineStart.distanceSquaredTo(lineEnd);
282  if (lineLengthSquared == 0.0f) {
283  return point.distanceTo(lineStart);
284  } else {
285  // scalar product equals length of orthogonal projection times length of vector being projected onto
286  // dividing the scalar product by the square of the distance gives the relative position
287  SUMOReal u = (((point.x() - lineStart.x()) * (lineEnd.x() - lineStart.x())) +
288  ((point.y() - lineStart.y()) * (lineEnd.y() - lineStart.y()))
289  ) / lineLengthSquared;
290  if (u < 0.0f || u > 1.0f) {
291  return -1; // closest point does not fall within the line segment
292  }
293  Position intersection(
294  lineStart.x() + u * (lineEnd.x() - lineStart.x()),
295  lineStart.y() + u * (lineEnd.y() - lineStart.y()));
296  return point.distanceTo(intersection);
297  }
298 }
299 
300 
301 SUMOReal
303  const Position& lineStart,
304  const Position& lineEnd,
305  Position& outIntersection) {
306  const SUMOReal length = nearest_offset_on_line_to_point2D(lineStart, lineEnd, point, false);
307  outIntersection.set(Line(lineStart, lineEnd).getPositionAtDistance(length));
308  return point.distanceTo2D(outIntersection);
309 }
310 
311 
312 
313 Position
315  const Position& lineBeg,
316  const Position& lineEnd,
317  SUMOReal amount) {
318  const SUMOReal dx = lineBeg.x() - lineEnd.x();
319  const SUMOReal dy = lineBeg.y() - lineEnd.y();
320  const SUMOReal length = sqrt(dx * dx + dy * dy);
321  if (length > 0) {
322  p.add(dy * amount / length, -dx * amount / length);
323  }
324  return p;
325 }
326 
327 
328 
329 Position
331  if (v.intersects(Position(b.xmin(), b.ymin()), Position(b.xmin(), b.ymax()))) {
332  return v.intersectsAtPoint(
333  Position(b.xmin(), b.ymin()),
334  Position(b.xmin(), b.ymax()));
335  }
336  if (v.intersects(Position(b.xmax(), b.ymin()), Position(b.xmax(), b.ymax()))) {
337  return v.intersectsAtPoint(
338  Position(b.xmax(), b.ymin()),
339  Position(b.xmax(), b.ymax()));
340  }
341  if (v.intersects(Position(b.xmin(), b.ymin()), Position(b.xmax(), b.ymin()))) {
342  return v.intersectsAtPoint(
343  Position(b.xmin(), b.ymin()),
344  Position(b.xmax(), b.ymin()));
345  }
346  if (v.intersects(Position(b.xmin(), b.ymax()), Position(b.xmax(), b.ymax()))) {
347  return v.intersectsAtPoint(
348  Position(b.xmin(), b.ymax()),
349  Position(b.xmax(), b.ymax()));
350  }
351  throw 1;
352 }
353 
354 std::pair<SUMOReal, SUMOReal>
356  const Position& end,
357  SUMOReal wanted_offset) {
358  return getNormal90D_CW(beg, end, beg.distanceTo2D(end), wanted_offset);
359 }
360 
361 
362 std::pair<SUMOReal, SUMOReal>
364  const Position& end,
365  SUMOReal length, SUMOReal wanted_offset) {
366  if (beg == end) {
367  throw InvalidArgument("same points at " + toString(beg));
368  }
369  return std::pair<SUMOReal, SUMOReal>
370  ((beg.y() - end.y()) * wanted_offset / length, (end.x() - beg.x()) * wanted_offset / length);
371 }
372 
373 
374 SUMOReal
376  SUMOReal v = angle2 - angle1;
377  if (v < 0) {
378  v = 360 + v;
379  }
380  return v;
381 }
382 
383 
384 SUMOReal
386  SUMOReal v = angle1 - angle2;
387  if (v < 0) {
388  v = 360 + v;
389  }
390  return v;
391 }
392 
393 
394 SUMOReal
396  return MIN2(getCWAngleDiff(angle1, angle2), getCCWAngleDiff(angle1, angle2));
397 }
398 
399 
400 SUMOReal
402  return MAX2(getCWAngleDiff(angle1, angle2), getCCWAngleDiff(angle1, angle2));
403 }
404 
405 
406 
407 /****************************************************************************/
408