SUMO - Simulation of Urban MObility
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Modules Pages
PositionVector.cpp
Go to the documentation of this file.
1 /****************************************************************************/
10 // A list of positions
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 <queue>
35 #include <cmath>
36 #include <iostream>
37 #include <algorithm>
38 #include <cassert>
39 #include <iterator>
40 #include <limits>
41 #include <utils/common/StdDefs.h>
43 #include <utils/common/ToString.h>
44 #include "AbstractPoly.h"
45 #include "Position.h"
46 #include "PositionVector.h"
47 #include "GeomHelper.h"
48 #include "Helper_ConvexHull.h"
49 #include "Boundary.h"
50 
51 #ifdef CHECK_MEMORY_LEAKS
52 #include <foreign/nvwa/debug_new.h>
53 #endif // CHECK_MEMORY_LEAKS
54 
55 // ===========================================================================
56 // method definitions
57 // ===========================================================================
58 
60 
61 
62 PositionVector::PositionVector(const std::vector<Position>& v) {
63  std::copy(v.begin(), v.end(), std::back_inserter(*this));
64 }
65 
66 
67 PositionVector::PositionVector(const std::vector<Position>::const_iterator beg, const std::vector<Position>::const_iterator end) {
68  std::copy(beg, end, std::back_inserter(*this));
69 }
70 
71 
73  push_back(p1);
74  push_back(p2);
75 }
76 
77 
79 
80 
81 bool
82 PositionVector::around(const Position& p, SUMOReal offset) const {
83  if (offset != 0) {
84  PositionVector tmp(*this);
85  tmp.scaleAbsolute(offset);
86  return tmp.around(p);
87  }
88  SUMOReal angle = 0;
89  for (const_iterator i = begin(); i != end() - 1; i++) {
90  Position p1(
91  (*i).x() - p.x(),
92  (*i).y() - p.y());
93  Position p2(
94  (*(i + 1)).x() - p.x(),
95  (*(i + 1)).y() - p.y());
96  angle += GeomHelper::angle2D(p1, p2);
97  }
98  Position p1(
99  (*(end() - 1)).x() - p.x(),
100  (*(end() - 1)).y() - p.y());
101  Position p2(
102  (*(begin())).x() - p.x(),
103  (*(begin())).y() - p.y());
104  angle += GeomHelper::angle2D(p1, p2);
105  return (!(fabs(angle) < M_PI));
106 }
107 
108 
109 bool
111  for (const_iterator i = begin(); i != end() - 1; i++) {
112  if (poly.around(*i, offset)) {
113  return true;
114  }
115  }
116  return false;
117 }
118 
119 
120 bool
121 PositionVector::intersects(const Position& p1, const Position& p2) const {
122  if (size() < 2) {
123  return false;
124  }
125  for (const_iterator i = begin(); i != end() - 1; i++) {
126  if (intersects(*i, *(i + 1), p1, p2)) {
127  return true;
128  }
129  }
130  return false;
131 }
132 
133 
134 bool
136  if (size() < 2) {
137  return false;
138  }
139  for (const_iterator i = begin(); i != end() - 1; i++) {
140  if (v1.intersects(*i, *(i + 1))) {
141  return true;
142  }
143  }
144  return false;
145 }
146 
147 
148 Position
149 PositionVector::intersectionPosition2D(const Position& p1, const Position& p2, const SUMOReal withinDist) const {
150  for (const_iterator i = begin(); i != end() - 1; i++) {
151  SUMOReal x, y, m;
152  if (intersects(*i, *(i + 1), p1, p2, withinDist, &x, &y, &m)) {
153  return Position(x, y);
154  }
155  }
156  return Position::INVALID;
157 }
158 
159 
160 Position
162  for (const_iterator i = begin(); i != end() - 1; i++) {
163  if (v1.intersects(*i, *(i + 1))) {
164  return v1.intersectionPosition2D(*i, *(i + 1));
165  }
166  }
167  return Position::INVALID;
168 }
169 
170 
171 const Position&
172 PositionVector::operator[](int index) const {
173  if (index >= 0) {
174  return at(index);
175  } else {
176  return at((int)size() + index);
177  }
178 }
179 
180 
181 Position&
183  if (index >= 0) {
184  return at(index);
185  } else {
186  return at((int)size() + index);
187  }
188 }
189 
190 
191 Position
193  const_iterator i = begin();
194  SUMOReal seenLength = 0;
195  do {
196  const SUMOReal nextLength = (*i).distanceTo(*(i + 1));
197  if (seenLength + nextLength > pos) {
198  return positionAtOffset(*i, *(i + 1), pos - seenLength, lateralOffset);
199  }
200  seenLength += nextLength;
201  } while (++i != end() - 1);
202  return back();
203 }
204 
205 
206 Position
208  const_iterator i = begin();
209  SUMOReal seenLength = 0;
210  do {
211  const SUMOReal nextLength = (*i).distanceTo2D(*(i + 1));
212  if (seenLength + nextLength > pos) {
213  return positionAtOffset2D(*i, *(i + 1), pos - seenLength, lateralOffset);
214  }
215  seenLength += nextLength;
216  } while (++i != end() - 1);
217  return back();
218 }
219 
220 
221 SUMOReal
223  if (pos < 0) {
224  pos += length();
225  }
226  const_iterator i = begin();
227  SUMOReal seenLength = 0;
228  do {
229  const Position& p1 = *i;
230  const Position& p2 = *(i + 1);
231  const SUMOReal nextLength = p1.distanceTo(p2);
232  if (seenLength + nextLength > pos) {
233  return p1.angleTo2D(p2);
234  }
235  seenLength += nextLength;
236  } while (++i != end() - 1);
237  const Position& p1 = (*this)[-2];
238  const Position& p2 = back();
239  return p1.angleTo2D(p2);
240 }
241 
242 
243 SUMOReal
246 }
247 
248 
249 SUMOReal
251  const_iterator i = begin();
252  SUMOReal seenLength = 0;
253  do {
254  const Position& p1 = *i;
255  const Position& p2 = *(i + 1);
256  const SUMOReal nextLength = p1.distanceTo(p2);
257  if (seenLength + nextLength > pos) {
258  return RAD2DEG(atan2(p2.z() - p1.z(), p1.distanceTo2D(p2)));
259  }
260  seenLength += nextLength;
261  } while (++i != end() - 1);
262  const Position& p1 = (*this)[-2];
263  const Position& p2 = back();
264  return RAD2DEG(atan2(p2.z() - p1.z(), p1.distanceTo2D(p2)));
265 }
266 
267 
268 Position
269 PositionVector::positionAtOffset(const Position& p1, const Position& p2, SUMOReal pos, SUMOReal lateralOffset) {
270  const SUMOReal dist = p1.distanceTo(p2);
271  if (pos < 0 || dist < pos) {
272  return Position::INVALID;
273  }
274  if (lateralOffset != 0) {
275  const Position offset = sideOffset(p1, p2, -lateralOffset); // move in the same direction as Position::move2side
276  if (pos == 0.) {
277  return p1 + offset;
278  }
279  return p1 + (p2 - p1) * (pos / dist) + offset;
280  }
281  if (pos == 0.) {
282  return p1;
283  }
284  return p1 + (p2 - p1) * (pos / dist);
285 }
286 
287 
288 Position
289 PositionVector::positionAtOffset2D(const Position& p1, const Position& p2, SUMOReal pos, SUMOReal lateralOffset) {
290  const SUMOReal dist = p1.distanceTo2D(p2);
291  if (pos < 0 || dist < pos) {
292  return Position::INVALID;
293  }
294  if (lateralOffset != 0) {
295  const Position offset = sideOffset(p1, p2, -lateralOffset); // move in the same direction as Position::move2side
296  if (pos == 0.) {
297  return p1 + offset;
298  }
299  return p1 + (p2 - p1) * (pos / dist) + offset;
300  }
301  if (pos == 0.) {
302  return p1;
303  }
304  return p1 + (p2 - p1) * (pos / dist);
305 }
306 
307 
308 Boundary
310  Boundary ret;
311  for (const_iterator i = begin(); i != end(); i++) {
312  ret.add(*i);
313  }
314  return ret;
315 }
316 
317 
318 Position
320  SUMOReal x = 0;
321  SUMOReal y = 0;
322  SUMOReal z = 0;
323  for (const_iterator i = begin(); i != end(); i++) {
324  x += (*i).x();
325  y += (*i).y();
326  z += (*i).z();
327  }
328  return Position(x / (SUMOReal) size(), y / (SUMOReal) size(), z / (SUMOReal)size());
329 }
330 
331 
332 Position
334  PositionVector tmp = *this;
335  if (!isClosed()) { // make sure its closed
336  tmp.push_back(tmp[0]);
337  }
338  const int endIndex = (int)tmp.size() - 1;
339  SUMOReal div = 0; // 6 * area including sign
340  SUMOReal x = 0;
341  SUMOReal y = 0;
342  if (tmp.area() != 0) { // numerical instability ?
343  // http://en.wikipedia.org/wiki/Polygon
344  for (int i = 0; i < endIndex; i++) {
345  const SUMOReal z = tmp[i].x() * tmp[i + 1].y() - tmp[i + 1].x() * tmp[i].y();
346  div += z; // area formula
347  x += (tmp[i].x() + tmp[i + 1].x()) * z;
348  y += (tmp[i].y() + tmp[i + 1].y()) * z;
349  }
350  div *= 3; // 6 / 2, the 2 comes from the area formula
351  return Position(x / div, y / div);
352  } else {
353  // compute by decomposing into line segments
354  // http://en.wikipedia.org/wiki/Centroid#By_geometric_decomposition
355  SUMOReal lengthSum = 0;
356  for (int i = 0; i < endIndex; i++) {
357  SUMOReal length = tmp[i].distanceTo(tmp[i + 1]);
358  x += (tmp[i].x() + tmp[i + 1].x()) * length / 2;
359  y += (tmp[i].y() + tmp[i + 1].y()) * length / 2;
360  lengthSum += length;
361  }
362  if (lengthSum == 0) {
363  // it is probably only one point
364  return tmp[0];
365  }
366  return Position(x / lengthSum, y / lengthSum);
367  }
368 }
369 
370 
371 void
373  Position centroid = getCentroid();
374  for (int i = 0; i < static_cast<int>(size()); i++) {
375  (*this)[i] = centroid + (((*this)[i] - centroid) * factor);
376  }
377 }
378 
379 
380 void
382  Position centroid = getCentroid();
383  for (int i = 0; i < static_cast<int>(size()); i++) {
384  (*this)[i] = centroid + (((*this)[i] - centroid) + offset);
385  }
386 }
387 
388 
389 Position
391  if (size() == 1) {
392  return (*this)[0];
393  }
394  return positionAtOffset(SUMOReal((length() / 2.)));
395 }
396 
397 
398 SUMOReal
400  SUMOReal len = 0;
401  for (const_iterator i = begin(); i != end() - 1; i++) {
402  len += (*i).distanceTo(*(i + 1));
403  }
404  return len;
405 }
406 
407 
408 SUMOReal
410  SUMOReal len = 0;
411  for (const_iterator i = begin(); i != end() - 1; i++) {
412  len += (*i).distanceTo2D(*(i + 1));
413  }
414  return len;
415 }
416 
417 
418 SUMOReal
420  if (size() < 3) {
421  return 0;
422  }
423  SUMOReal area = 0;
424  PositionVector tmp = *this;
425  if (!isClosed()) { // make sure its closed
426  tmp.push_back(tmp[0]);
427  }
428  const int endIndex = (int)tmp.size() - 1;
429  // http://en.wikipedia.org/wiki/Polygon
430  for (int i = 0; i < endIndex; i++) {
431  area += tmp[i].x() * tmp[i + 1].y() - tmp[i + 1].x() * tmp[i].y();
432  }
433  if (area < 0) { // we whether we had cw or ccw order
434  area *= -1;
435  }
436  return area / 2;
437 }
438 
439 
440 bool
442  for (const_iterator i = begin(); i != end() - 1; i++) {
443  if (poly.around(*i, offset)) {
444  return true;
445  }
446  }
447  return false;
448 }
449 
450 
451 bool
452 PositionVector::crosses(const Position& p1, const Position& p2) const {
453  return intersects(p1, p2);
454 }
455 
456 
457 std::pair<PositionVector, PositionVector>
459  if (size() < 2) {
460  throw InvalidArgument("Vector to short for splitting");
461  }
462  if (where <= POSITION_EPS || where >= length() - POSITION_EPS) {
463  WRITE_WARNING("Splitting vector close to end (pos: " + toString(where) + ", length: " + toString(length()) + ")");
464  }
465  PositionVector first, second;
466  first.push_back((*this)[0]);
467  SUMOReal seen = 0;
468  const_iterator it = begin() + 1;
469  SUMOReal next = first.back().distanceTo(*it);
470  // see how many points we can add to first
471  while (where >= seen + next + POSITION_EPS) {
472  seen += next;
473  first.push_back(*it);
474  it++;
475  next = first.back().distanceTo(*it);
476  }
477  if (fabs(where - (seen + next)) > POSITION_EPS || it == end() - 1) {
478  // we need to insert a new point because 'where' is not close to an
479  // existing point or it is to close to the endpoint
480  const Position p = positionAtOffset(first.back(), *it, where - seen);
481  first.push_back(p);
482  second.push_back(p);
483  } else {
484  first.push_back(*it);
485  }
486  // add the remaining points to second
487  for (; it != end(); it++) {
488  second.push_back(*it);
489  }
490  assert(first.size() >= 2);
491  assert(second.size() >= 2);
492  assert(first.back() == second.front());
493  assert(fabs(first.length() + second.length() - length()) < 2 * POSITION_EPS);
494  return std::pair<PositionVector, PositionVector>(first, second);
495 }
496 
497 
498 std::ostream&
499 operator<<(std::ostream& os, const PositionVector& geom) {
500  for (PositionVector::const_iterator i = geom.begin(); i != geom.end(); i++) {
501  if (i != geom.begin()) {
502  os << " ";
503  }
504  os << (*i);
505  }
506  return os;
507 }
508 
509 
510 void
512  std::sort(begin(), end(), as_poly_cw_sorter());
513 }
514 
515 
516 void
518  for (int i = 0; i < static_cast<int>(size()); i++) {
519  (*this)[i].add(xoff, yoff, zoff);
520  }
521 }
522 
523 
524 void
526  add(offset.x(), offset.y(), offset.z());
527 }
528 
529 
530 void
532  for (int i = 0; i < static_cast<int>(size()); i++) {
533  (*this)[i].mul(1, -1);
534  }
535 }
536 
537 
539 
540 
541 int
543  return atan2(p1.x(), p1.y()) < atan2(p2.x(), p2.y());
544 }
545 
546 
547 void
549  std::sort(begin(), end(), increasing_x_y_sorter());
550 }
551 
552 
554 
555 
556 int
558  const Position& p2) const {
559  if (p1.x() != p2.x()) {
560  return p1.x() < p2.x();
561  }
562  return p1.y() < p2.y();
563 }
564 
565 
566 SUMOReal
567 PositionVector::isLeft(const Position& P0, const Position& P1, const Position& P2) const {
568  return (P1.x() - P0.x()) * (P2.y() - P0.y()) - (P2.x() - P0.x()) * (P1.y() - P0.y());
569 }
570 
571 
574  PositionVector ret = *this;
575  ret.sortAsPolyCWByAngle();
576  return simpleHull_2D(ret);
577 }
578 
579 
580 void
582  if (size() > 0 && v.size() > 0 && back().distanceTo(v[0]) < sameThreshold) {
583  copy(v.begin() + 1, v.end(), back_inserter(*this));
584  } else {
585  copy(v.begin(), v.end(), back_inserter(*this));
586  }
587 }
588 
589 
591 PositionVector::getSubpart(SUMOReal beginOffset, SUMOReal endOffset) const {
592  PositionVector ret;
593  Position begPos = front();
594  if (beginOffset > POSITION_EPS) {
595  begPos = positionAtOffset(beginOffset);
596  }
597  Position endPos = back();
598  if (endOffset < length() - POSITION_EPS) {
599  endPos = positionAtOffset(endOffset);
600  }
601  ret.push_back(begPos);
602 
603  SUMOReal seen = 0;
604  const_iterator i = begin();
605  // skip previous segments
606  while ((i + 1) != end()
607  &&
608  seen + (*i).distanceTo(*(i + 1)) < beginOffset) {
609  seen += (*i).distanceTo(*(i + 1));
610  i++;
611  }
612  // append segments in between
613  while ((i + 1) != end()
614  &&
615  seen + (*i).distanceTo(*(i + 1)) < endOffset) {
616 
617  ret.push_back_noDoublePos(*(i + 1));
618  seen += (*i).distanceTo(*(i + 1));
619  i++;
620  }
621  // append end
622  ret.push_back_noDoublePos(endPos);
623  return ret;
624 }
625 
626 
628 PositionVector::getSubpart2D(SUMOReal beginOffset, SUMOReal endOffset) const {
629  PositionVector ret;
630  Position begPos = front();
631  if (beginOffset > POSITION_EPS) {
632  begPos = positionAtOffset2D(beginOffset);
633  }
634  Position endPos = back();
635  if (endOffset < length2D() - POSITION_EPS) {
636  endPos = positionAtOffset2D(endOffset);
637  }
638  ret.push_back(begPos);
639 
640  SUMOReal seen = 0;
641  const_iterator i = begin();
642  // skip previous segments
643  while ((i + 1) != end()
644  &&
645  seen + (*i).distanceTo2D(*(i + 1)) < beginOffset) {
646  seen += (*i).distanceTo2D(*(i + 1));
647  i++;
648  }
649  // append segments in between
650  while ((i + 1) != end()
651  &&
652  seen + (*i).distanceTo2D(*(i + 1)) < endOffset) {
653 
654  ret.push_back_noDoublePos(*(i + 1));
655  seen += (*i).distanceTo2D(*(i + 1));
656  i++;
657  }
658  // append end
659  ret.push_back_noDoublePos(endPos);
660  return ret;
661 }
662 
663 
665 PositionVector::getSubpartByIndex(int beginIndex, int count) const {
666  if (beginIndex < 0) {
667  beginIndex += (int)size();
668  }
669  assert(count >= 0);
670  assert(beginIndex < (int)size());
671  assert(beginIndex + count <= (int)size());
672  PositionVector result;
673  for (int i = beginIndex; i < beginIndex + count; ++i) {
674  result.push_back((*this)[i]);
675  }
676  return result;
677 }
678 
679 
680 SUMOReal
682  return front().angleTo2D(back());
683 }
684 
685 
686 SUMOReal
687 PositionVector::nearest_offset_to_point2D(const Position& p, bool perpendicular) const {
690  SUMOReal seen = 0;
691  for (const_iterator i = begin(); i != end() - 1; i++) {
692  const SUMOReal pos =
693  GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, perpendicular);
694  const SUMOReal dist = pos == GeomHelper::INVALID_OFFSET ? minDist : p.distanceTo2D(positionAtOffset2D(*i, *(i + 1), pos));
695  if (dist < minDist) {
696  nearestPos = pos + seen;
697  minDist = dist;
698  }
699  if (perpendicular && i != begin() && pos == GeomHelper::INVALID_OFFSET) {
700  // even if perpendicular is set we still need to check the distance to the inner points
701  const SUMOReal cornerDist = p.distanceTo2D(*i);
702  if (cornerDist < minDist) {
703  const SUMOReal pos1 =
704  GeomHelper::nearest_offset_on_line_to_point2D(*(i - 1), *i, p, false);
705  const SUMOReal pos2 =
706  GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, false);
707  if (pos1 == (*(i - 1)).distanceTo2D(*i) && pos2 == 0.) {
708  nearestPos = seen;
709  minDist = cornerDist;
710  }
711  }
712  }
713  seen += (*i).distanceTo2D(*(i + 1));
714  }
715  return nearestPos;
716 }
717 
718 
719 Position
721  // @toDo this duplicates most of the code in nearest_offset_to_point2D. It should be refactored
722  if (extend) {
723  PositionVector extended = *this;
724  const SUMOReal dist = 2 * distance2D(p);
725  extended.extrapolate(dist);
726  return extended.transformToVectorCoordinates(p) - Position(dist, 0);
727  }
729  SUMOReal nearestPos = -1;
730  SUMOReal seen = 0;
731  int sign = 1;
732  for (const_iterator i = begin(); i != end() - 1; i++) {
733  const SUMOReal pos =
734  GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, true);
735  const SUMOReal dist = pos < 0 ? minDist : p.distanceTo2D(positionAtOffset(*i, *(i + 1), pos));
736  if (dist < minDist) {
737  nearestPos = pos + seen;
738  minDist = dist;
739  sign = isLeft(*i, *(i + 1), p) >= 0 ? -1 : 1;
740  }
741  if (i != begin() && pos == GeomHelper::INVALID_OFFSET) {
742  // even if perpendicular is set we still need to check the distance to the inner points
743  const SUMOReal cornerDist = p.distanceTo2D(*i);
744  if (cornerDist < minDist) {
745  const SUMOReal pos1 =
746  GeomHelper::nearest_offset_on_line_to_point2D(*(i - 1), *i, p, false);
747  const SUMOReal pos2 =
748  GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, false);
749  if (pos1 == (*(i - 1)).distanceTo2D(*i) && pos2 == 0.) {
750  nearestPos = seen;
751  minDist = cornerDist;
752  sign = isLeft(*(i - 1), *i, p) >= 0 ? -1 : 1;
753  }
754  }
755  }
756  seen += (*i).distanceTo2D(*(i + 1));
757  }
758  if (nearestPos != -1) {
759  return Position(nearestPos, sign * minDist);
760  } else {
761  return Position::INVALID;
762  }
763 }
764 
765 
766 int
768  assert(size() > 0);
770  SUMOReal dist;
771  int closest = 0;
772  for (int i = 0; i < (int)size(); i++) {
773  dist = p.distanceTo((*this)[i]);
774  if (dist < minDist) {
775  closest = i;
776  minDist = dist;
777  }
778  }
779  return closest;
780 }
781 
782 
783 int
786  int insertionIndex = 1;
787  for (int i = 0; i < (int)size() - 1; i++) {
788  const SUMOReal length = GeomHelper::nearest_offset_on_line_to_point2D((*this)[i], (*this)[i + 1], p, false);
789  const Position& outIntersection = PositionVector::positionAtOffset2D((*this)[i], (*this)[i + 1], length);
790  const SUMOReal dist = p.distanceTo2D(outIntersection);
791  if (dist < minDist) {
792  insertionIndex = i + 1;
793  minDist = dist;
794  }
795  }
796  insert(begin() + insertionIndex, p);
797  return insertionIndex;
798 }
799 
800 
801 int
803  if (size() == 0) {
804  return -1;
805  }
807  int removalIndex = 0;
808  for (int i = 0; i < (int)size(); i++) {
809  const SUMOReal dist = p.distanceTo2D((*this)[i]);
810  if (dist < minDist) {
811  removalIndex = i;
812  minDist = dist;
813  }
814  }
815  erase(begin() + removalIndex);
816  return removalIndex;
817 }
818 
819 
820 std::vector<SUMOReal>
822  std::vector<SUMOReal> ret;
823  for (const_iterator i = other.begin(); i != other.end() - 1; i++) {
824  std::vector<SUMOReal> atSegment = intersectsAtLengths2D(*i, *(i + 1));
825  copy(atSegment.begin(), atSegment.end(), back_inserter(ret));
826  }
827  return ret;
828 }
829 
830 
831 std::vector<SUMOReal>
833  std::vector<SUMOReal> ret;
834  SUMOReal pos = 0;
835  for (const_iterator i = begin(); i != end() - 1; i++) {
836  const Position& p1 = *i;
837  const Position& p2 = *(i + 1);
838  SUMOReal x, y, m;
839  if (intersects(p1, p2, lp1, lp2, 0., &x, &y, &m)) {
840  ret.push_back(Position(x, y).distanceTo2D(p1) + pos);
841  }
842  pos += p1.distanceTo2D(p2);
843  }
844  return ret;
845 }
846 
847 
848 void
849 PositionVector::extrapolate(const SUMOReal val, const bool onlyFirst) {
850  assert(size() > 1);
851  Position& p1 = (*this)[0];
852  Position& p2 = (*this)[1];
853  const Position offset = (p2 - p1) * (val / p1.distanceTo(p2));
854  p1.sub(offset);
855  if (!onlyFirst) {
856  if (size() == 2) {
857  p2.add(offset);
858  } else {
859  const Position& e1 = (*this)[-2];
860  Position& e2 = (*this)[-1];
861  e2.sub((e1 - e2) * (val / e1.distanceTo(e2)));
862  }
863  }
864 }
865 
866 
867 void
868 PositionVector::extrapolate2D(const SUMOReal val, const bool onlyFirst) {
869  assert(size() > 1);
870  Position& p1 = (*this)[0];
871  Position& p2 = (*this)[1];
872  const Position offset = (p2 - p1) * (val / p1.distanceTo2D(p2));
873  p1.sub(offset);
874  if (!onlyFirst) {
875  if (size() == 2) {
876  p2.add(offset);
877  } else {
878  const Position& e1 = (*this)[-2];
879  Position& e2 = (*this)[-1];
880  e2.sub((e1 - e2) * (val / e1.distanceTo2D(e2)));
881  }
882  }
883 }
884 
885 
888  PositionVector ret;
889  for (const_reverse_iterator i = rbegin(); i != rend(); i++) {
890  ret.push_back(*i);
891  }
892  return ret;
893 }
894 
895 
896 Position
897 PositionVector::sideOffset(const Position& beg, const Position& end, const SUMOReal amount) {
898  const SUMOReal scale = amount / beg.distanceTo2D(end);
899  return Position((beg.y() - end.y()) * scale, (end.x() - beg.x()) * scale);
900 }
901 
902 
903 void
905  if (size() < 2) {
906  return;
907  }
908  if (length2D() == 0) {
909  return;
910  }
911  PositionVector shape;
912  for (int i = 0; i < static_cast<int>(size()); i++) {
913  if (i == 0) {
914  const Position& from = (*this)[i];
915  const Position& to = (*this)[i + 1];
916  shape.push_back(from - sideOffset(from, to, amount));
917  } else if (i == static_cast<int>(size()) - 1) {
918  const Position& from = (*this)[i - 1];
919  const Position& to = (*this)[i];
920  shape.push_back(to - sideOffset(from, to, amount));
921  } else {
922  const Position& from = (*this)[i - 1];
923  const Position& me = (*this)[i];
924  const Position& to = (*this)[i + 1];
925  PositionVector fromMe(from, me);
926  fromMe.extrapolate2D(me.distanceTo2D(to));
927  const SUMOReal extrapolateDev = fromMe[1].distanceTo2D(to);
928  if (fabs(extrapolateDev) < POSITION_EPS) {
929  // parallel case, just shift the middle point
930  shape.push_back(me - sideOffset(from, to, amount));
931  } else if (fabs(extrapolateDev - 2 * me.distanceTo2D(to)) < POSITION_EPS) {
932  // counterparallel case, just shift the middle point
933  PositionVector fromMe(from, me);
934  fromMe.extrapolate2D(amount);
935  shape.push_back(fromMe[1]);
936  } else {
937  Position offsets = sideOffset(from, me, amount);
938  Position offsets2 = sideOffset(me, to, amount);
939  PositionVector l1(from - offsets, me - offsets);
940  PositionVector l2(me - offsets2, to - offsets2);
941  shape.push_back(l1.intersectionPosition2D(l2[0], l2[1], 100));
942  if (shape.back() == Position::INVALID) {
943  throw InvalidArgument("no line intersection");
944  }
945  }
946  // copy original z value
947  shape.back().set(shape.back().x(), shape.back().y(), me.z());
948  }
949  }
950  *this = shape;
951 }
952 
953 
954 SUMOReal
956  assert((int)size() > pos + 1);
957  return (*this)[pos].angleTo2D((*this)[pos + 1]);
958 }
959 
960 
961 void
963  if (size() == 0 || (*this)[0] == back()) {
964  return;
965  }
966  push_back((*this)[0]);
967 }
968 
969 
970 std::vector<SUMOReal>
971 PositionVector::distances(const PositionVector& s, bool perpendicular) const {
972  std::vector<SUMOReal> ret;
973  const_iterator i;
974  for (i = begin(); i != end(); i++) {
975  const SUMOReal dist = s.distance2D(*i, perpendicular);
976  if (dist != GeomHelper::INVALID_OFFSET) {
977  ret.push_back(dist);
978  }
979  }
980  for (i = s.begin(); i != s.end(); i++) {
981  const SUMOReal dist = distance2D(*i, perpendicular);
982  if (dist != GeomHelper::INVALID_OFFSET) {
983  ret.push_back(dist);
984  }
985  }
986  return ret;
987 }
988 
989 
990 SUMOReal
991 PositionVector::distance2D(const Position& p, bool perpendicular) const {
992  if (size() == 0) {
994  } else if (size() == 1) {
995  return front().distanceTo(p);
996  }
997  const SUMOReal nearestOffset = nearest_offset_to_point2D(p, perpendicular);
998  if (nearestOffset == GeomHelper::INVALID_OFFSET) {
1000  } else {
1001  return p.distanceTo2D(positionAtOffset2D(nearestOffset));
1002  }
1003 }
1004 
1005 
1006 void
1008  if (size() == 0 || !p.almostSame(back())) {
1009  push_back(p);
1010  }
1011 }
1012 
1013 
1014 void
1016  if (size() == 0 || !p.almostSame(front())) {
1017  insert(begin(), p);
1018  }
1019 }
1020 
1021 
1022 bool
1024  return size() >= 2 && (*this)[0] == back();
1025 }
1026 
1027 
1028 void
1029 PositionVector::removeDoublePoints(SUMOReal minDist, bool assertLength) {
1030  if (size() > 1) {
1031  iterator last = begin();
1032  for (iterator i = begin() + 1; i != end() && (!assertLength || size() > 2);) {
1033  if (last->almostSame(*i, minDist)) {
1034  i = erase(i);
1035  } else {
1036  last = i;
1037  ++i;
1038  }
1039  }
1040  }
1041 }
1042 
1043 
1044 bool
1046  if (size() == v2.size()) {
1047  for (int i = 0; i < (int)size(); i++) {
1048  if ((*this)[i] != v2[i]) {
1049  return false;
1050  }
1051  }
1052  return true;
1053  } else {
1054  return false;
1055  }
1056 }
1057 
1058 
1059 bool
1061  if (size() > 2) {
1062  return false;
1063  }
1064  for (const_iterator i = begin(); i != end() - 1; i++) {
1065  if ((*i).z() != (*(i + 1)).z()) {
1066  return true;
1067  }
1068  }
1069  return false;
1070 }
1071 
1072 
1073 bool
1074 PositionVector::intersects(const Position& p11, const Position& p12, const Position& p21, const Position& p22, const SUMOReal withinDist, SUMOReal* x, SUMOReal* y, SUMOReal* mu) {
1075  const SUMOReal eps = std::numeric_limits<SUMOReal>::epsilon();
1076  const double denominator = (p22.y() - p21.y()) * (p12.x() - p11.x()) - (p22.x() - p21.x()) * (p12.y() - p11.y());
1077  const double numera = (p22.x() - p21.x()) * (p11.y() - p21.y()) - (p22.y() - p21.y()) * (p11.x() - p21.x());
1078  const double numerb = (p12.x() - p11.x()) * (p11.y() - p21.y()) - (p12.y() - p11.y()) * (p11.x() - p21.x());
1079  /* Are the lines coincident? */
1080  if (fabs(numera) < eps && fabs(numerb) < eps && fabs(denominator) < eps) {
1081  SUMOReal a1;
1082  SUMOReal a2;
1083  SUMOReal a3;
1084  SUMOReal a4;
1085  SUMOReal a = -1e12;
1086  if (p11.x() != p12.x()) {
1087  a1 = p11.x() < p12.x() ? p11.x() : p12.x();
1088  a2 = p11.x() < p12.x() ? p12.x() : p11.x();
1089  a3 = p21.x() < p22.x() ? p21.x() : p22.x();
1090  a4 = p21.x() < p22.x() ? p22.x() : p21.x();
1091  } else {
1092  a1 = p11.y() < p12.y() ? p11.y() : p12.y();
1093  a2 = p11.y() < p12.y() ? p12.y() : p11.y();
1094  a3 = p21.y() < p22.y() ? p21.y() : p22.y();
1095  a4 = p21.y() < p22.y() ? p22.y() : p21.y();
1096  }
1097  if (a1 <= a3 && a3 <= a2) {
1098  if (a4 < a2) {
1099  a = (a3 + a4) / 2;
1100  } else {
1101  a = (a2 + a3) / 2;
1102  }
1103  }
1104  if (a3 <= a1 && a1 <= a4) {
1105  if (a2 < a4) {
1106  a = (a1 + a2) / 2;
1107  } else {
1108  a = (a1 + a4) / 2;
1109  }
1110  }
1111  if (a != -1e12) {
1112  if (x != 0) {
1113  if (p11.x() != p12.x()) {
1114  *mu = (a - p11.x()) / (p12.x() - p11.x());
1115  *x = a;
1116  *y = p11.y() + (*mu) * (p12.y() - p11.y());
1117  } else {
1118  *x = p11.x();
1119  *y = a;
1120  if (p12.y() == p11.y()) {
1121  *mu = 0;
1122  } else {
1123  *mu = (a - p11.y()) / (p12.y() - p11.y());
1124  }
1125  }
1126  }
1127  return true;
1128  }
1129  return false;
1130  }
1131  /* Are the lines parallel */
1132  if (fabs(denominator) < eps) {
1133  return false;
1134  }
1135  /* Is the intersection along the segments */
1136  double mua = numera / denominator;
1137  /* reduce rounding errors for lines ending in the same point */
1138  if (fabs(p12.x() - p22.x()) < eps && fabs(p12.y() - p22.y()) < eps) {
1139  mua = 1.;
1140  } else {
1141  const double offseta = withinDist / p11.distanceTo2D(p12);
1142  const double offsetb = withinDist / p21.distanceTo2D(p22);
1143  const double mub = numerb / denominator;
1144  if (mua < -offseta || mua > 1 + offseta || mub < -offsetb || mub > 1 + offsetb) {
1145  return false;
1146  }
1147  }
1148  if (x != 0) {
1149  *x = p11.x() + mua * (p12.x() - p11.x());
1150  *y = p11.y() + mua * (p12.y() - p11.y());
1151  *mu = mua;
1152  }
1153  return true;
1154 }
1155 
1156 
1157 void
1159  const SUMOReal s = sin(angle);
1160  const SUMOReal c = cos(angle);
1161  for (int i = 0; i < (int)size(); i++) {
1162  const SUMOReal x = (*this)[i].x();
1163  const SUMOReal y = (*this)[i].y();
1164  const SUMOReal z = (*this)[i].z();
1165  const SUMOReal xnew = x * c - y * s;
1166  const SUMOReal ynew = x * s + y * c;
1167  (*this)[i].set(xnew, ynew, z);
1168  }
1169 }
1170 
1171 
1174  PositionVector result = *this;
1175  bool changed = true;
1176  while (changed && result.size() > 3) {
1177  changed = false;
1178  for (int i = 0; i < (int)result.size(); i++) {
1179  const Position& p1 = result[i];
1180  const Position& p2 = result[(i + 2) % result.size()];
1181  const int middleIndex = (i + 1) % result.size();
1182  const Position& p0 = result[middleIndex];
1183  // https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Line_defined_by_two_points
1184  const SUMOReal triangleArea2 = fabs((p2.y() - p1.y()) * p0.x() - (p2.x() - p1.x()) * p0.y() + p2.x() * p1.y() - p2.y() * p1.x());
1185  const SUMOReal distIK = p1.distanceTo2D(p2);
1186  if (distIK > NUMERICAL_EPS && triangleArea2 / distIK < NUMERICAL_EPS) {
1187  changed = true;
1188  result.erase(result.begin() + middleIndex);
1189  break;
1190  }
1191  }
1192  }
1193  return result;
1194 }
1195 
1196 
1198 PositionVector::getOrthogonal(const Position& p, SUMOReal extend, SUMOReal& distToClosest) const {
1199  PositionVector result;
1200  PositionVector tmp = *this;
1201  tmp.extrapolate2D(extend);
1202  const SUMOReal baseOffset = tmp.nearest_offset_to_point2D(p);
1203  if (baseOffset == GeomHelper::INVALID_OFFSET) {
1204  // fail
1205  return result;
1206  }
1207  Position base = tmp.positionAtOffset2D(baseOffset);
1208  distToClosest = tmp[tmp.indexOfClosest(base)].distanceTo2D(base);
1209  if (p.distanceTo2D(base) > NUMERICAL_EPS) {
1210  result.push_back(p);
1211  result.push_back(base);
1212  }
1213  return result;
1214 }
1215 
1216 /****************************************************************************/
1217 
void sub(SUMOReal dx, SUMOReal dy)
Substracts the given position from this one.
Definition: Position.h:139
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
clase for increasing Sorter
static Position sideOffset(const Position &beg, const Position &end, const SUMOReal amount)
get a side position of position vector using a offset
bool hasElevation() const
return whether two positions differ in z-coordinate
SUMOReal rotationAtOffset(SUMOReal pos) const
Returns the rotation at the given length.
SUMOReal nearest_offset_to_point2D(const Position &p, bool perpendicular=true) const
return the nearest offest to point 2D
PositionVector getSubpart2D(SUMOReal beginOffset, SUMOReal endOffset) const
get subpart of a position vector in two dimensions (Z is ignored)
void sortAsPolyCWByAngle()
short as polygon CV by angle
void add(const Position &pos)
Adds the given position to this one.
Definition: Position.h:119
PositionVector getOrthogonal(const Position &p, SUMOReal extend, SUMOReal &distToClosest) const
return orthogonal through p (extending this vector if necessary)
#define M_PI
Definition: angles.h:37
Position getCentroid() const
Returns the centroid (closes the polygon if unclosed)
bool intersects(const Position &p1, const Position &p2) const
Returns the information whether this list of points interesects the given line.
void scaleRelative(SUMOReal factor)
enlarges/shrinks the polygon by a factor based at the centroid
PositionVector getSubpartByIndex(int beginIndex, int count) const
get subpart of a position vector using index and a cout
bool partialWithin(const AbstractPoly &poly, SUMOReal offset=0) const
Returns the information whether this polygon lies partially within the given polygon.
#define RAD2DEG(x)
Definition: GeomHelper.h:46
SUMOReal distanceTo(const Position &p2) const
returns the euclidean distance in 3 dimension
Definition: Position.h:221
bool around(const Position &p, SUMOReal offset=0) const
Returns the information whether the position vector describes a polygon lying around the given point...
bool almostSame(const Position &p2, SUMOReal maxDiv=POSITION_EPS) const
Definition: Position.h:215
SUMOReal beginEndAngle() const
returns the angle in radians of the line connecting the first and the last position ...
bool isClosed() const
check if PositionVector is closed
const Position & operator[](int index) const
returns the constat position at the given index !!! exceptions?
SUMOReal x() const
Returns the x-position.
Definition: Position.h:63
Position positionAtOffset2D(SUMOReal pos, SUMOReal lateralOffset=0) const
Returns the position at the given length.
A class that stores a 2D geometrical boundary.
Definition: Boundary.h:48
#define WRITE_WARNING(msg)
Definition: MsgHandler.h:200
SUMOReal distance2D(const Position &p, bool perpendicular=false) const
closest 2D-distance to point p (or -1 if perpendicular is true and the point is beyond this vector) ...
PositionVector reverse() const
reverse position vector
SUMOReal slopeDegreeAtOffset(SUMOReal pos) const
Returns the slope at the given length.
void rotate2D(SUMOReal angle)
PositionVector convexHull() const
~PositionVector()
Destructor.
void extrapolate2D(const SUMOReal val, const bool onlyFirst=false)
extrapolate position vector in two dimensions (Z is ignored)
SUMOReal length2D() const
Returns the length.
std::vector< SUMOReal > distances(const PositionVector &s, bool perpendicular=false) const
distances of all my points to s and all of s points to myself
void push_front_noDoublePos(const Position &p)
insert in front a non double position
#define max(a, b)
Definition: polyfonts.c:65
PositionVector simplified() const
return the same shape with intermediate colinear points removed
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.
void add(SUMOReal xoff, SUMOReal yoff, SUMOReal zoff)
int indexOfClosest(const Position &p) const
index of the closest position to p
int operator()(const Position &p1, const Position &p2) const
comparing operation for sort
SUMOReal z() const
Returns the z-position.
Definition: Position.h:73
Position positionAtOffset(SUMOReal pos, SUMOReal lateralOffset=0) const
Returns the position at the given length.
#define POSITION_EPS
Definition: config.h:188
int insertAtClosest(const Position &p)
inserts p between the two closest positions and returns the insertion index
std::string toString(const T &t, std::streamsize accuracy=OUTPUT_ACCURACY)
Definition: ToString.h:55
void sortByIncreasingXY()
shory by increasing X-Y Psitions
bool operator==(const PositionVector &v2) const
comparing operation
std::pair< PositionVector, PositionVector > splitAt(SUMOReal where) const
Returns the two lists made when this list vector is splitted at the given point.
virtual bool around(const Position &p, SUMOReal offset=0) const =0
PositionVector()
Constructor. Creates an empty position vector.
SUMOReal length() const
Returns the length.
SUMOReal rotationDegreeAtOffset(SUMOReal pos) const
Returns the rotation at the given length.
PositionVector simpleHull_2D(const PositionVector &V)
#define sign(a)
Definition: polyfonts.c:68
void removeDoublePoints(SUMOReal minDist=POSITION_EPS, bool assertLength=false)
Removes positions if too near.
SUMOReal angleAt2D(int pos) const
get angle in certain position of position vector
void add(SUMOReal x, SUMOReal y, SUMOReal z=0)
Makes the boundary include the given coordinate.
Definition: Boundary.cpp:90
void scaleAbsolute(SUMOReal offset)
enlarges/shrinks the polygon by an absolute offset based at the centroid
SUMOReal y() const
Returns the y-position.
Definition: Position.h:68
bool overlapsWith(const AbstractPoly &poly, SUMOReal offset=0) const
Returns the information whether the given polygon overlaps with this.
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.
Position getLineCenter() const
get line center
SUMOReal distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
Definition: Position.h:232
void move2side(SUMOReal amount)
move position vector to side using certain ammount
#define SUMOReal
Definition: config.h:214
#define NUMERICAL_EPS
Definition: config.h:161
void push_back_noDoublePos(const Position &p)
insert in back a non double position
int operator()(const Position &p1, const Position &p2) const
comparing operation
Position getPolygonCenter() const
Returns the arithmetic of all corner points.
SUMOReal area() const
Returns the area (0 for non-closed)
std::vector< SUMOReal > intersectsAtLengths2D(const PositionVector &other) const
For all intersections between this vector and other, return the 2D-length of the subvector from this ...
void closePolygon()
ensures that the last position equals the first
Boundary getBoxBoundary() const
Returns a boundary enclosing this list of lines.
bool crosses(const Position &p1, const Position &p2) const
int removeClosest(const Position &p)
removes the point closest to p and return the removal index
PositionVector getSubpart(SUMOReal beginOffset, SUMOReal endOffset) const
get subpart of a position vector
SUMOReal angleTo2D(const Position &other) const
returns the angle in the plane of the vector pointing from here to the other position ...
Definition: Position.h:243
void append(const PositionVector &v, SUMOReal sameThreshold=2.0)
void extrapolate(const SUMOReal val, const bool onlyFirst=false)
extrapolate position vector
static const SUMOReal INVALID_OFFSET
a value to signify offsets outside the range of [0, Line.length()]
Definition: GeomHelper.h:59
SUMOReal isLeft(const Position &P0, const Position &P1, const Position &P2) const
get left
std::ostream & operator<<(std::ostream &os, const PositionVector &geom)
static const Position INVALID
Definition: Position.h:261
Position transformToVectorCoordinates(const Position &p, bool extend=false) const
return position p within the length-wise coordinate system defined by this position vector...