SUMO - Simulation of Urban MObility
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 // ===========================================================================
59 
60 
61 PositionVector::PositionVector(const std::vector<Position>& v) {
62  std::copy(v.begin(), v.end(), std::back_inserter(*this));
63 }
64 
65 
66 PositionVector::PositionVector(const std::vector<Position>::const_iterator beg, const std::vector<Position>::const_iterator end) {
67  std::copy(beg, end, std::back_inserter(*this));
68 }
69 
70 
72  push_back(p1);
73  push_back(p2);
74 }
75 
76 
78 
79 
80 bool
81 PositionVector::around(const Position& p, SUMOReal offset) const {
82  if (offset != 0) {
83  PositionVector tmp(*this);
84  tmp.scaleAbsolute(offset);
85  return tmp.around(p);
86  }
87  SUMOReal angle = 0;
88  for (const_iterator i = begin(); i != end() - 1; i++) {
89  Position p1(
90  (*i).x() - p.x(),
91  (*i).y() - p.y());
92  Position p2(
93  (*(i + 1)).x() - p.x(),
94  (*(i + 1)).y() - p.y());
95  angle += GeomHelper::angle2D(p1, p2);
96  }
97  Position p1(
98  (*(end() - 1)).x() - p.x(),
99  (*(end() - 1)).y() - p.y());
100  Position p2(
101  (*(begin())).x() - p.x(),
102  (*(begin())).y() - p.y());
103  angle += GeomHelper::angle2D(p1, p2);
104  return (!(fabs(angle) < M_PI));
105 }
106 
107 
108 bool
110  for (const_iterator i = begin(); i != end() - 1; i++) {
111  if (poly.around(*i, offset)) {
112  return true;
113  }
114  }
115  return false;
116 }
117 
118 
119 bool
120 PositionVector::intersects(const Position& p1, const Position& p2) const {
121  if (size() < 2) {
122  return false;
123  }
124  for (const_iterator i = begin(); i != end() - 1; i++) {
125  if (intersects(*i, *(i + 1), p1, p2)) {
126  return true;
127  }
128  }
129  return false;
130 }
131 
132 
133 bool
135  if (size() < 2) {
136  return false;
137  }
138  for (const_iterator i = begin(); i != end() - 1; i++) {
139  if (v1.intersects(*i, *(i + 1))) {
140  return true;
141  }
142  }
143  return false;
144 }
145 
146 
147 Position
149  const Position& p2,
150  const SUMOReal withinDist) const {
151  for (const_iterator i = begin(); i != end() - 1; i++) {
152  SUMOReal x, y, m;
153  if (intersects(*i, *(i + 1), p1, p2, withinDist, &x, &y, &m)) {
154  return Position(x, y);
155  }
156  }
157  return Position::INVALID;
158 }
159 
160 
161 Position
163  for (const_iterator i = begin(); i != end() - 1; i++) {
164  if (v1.intersects(*i, *(i + 1))) {
165  return v1.intersectionPosition2D(*i, *(i + 1));
166  }
167  }
168  return Position::INVALID;
169 }
170 
171 
172 const Position&
173 PositionVector::operator[](int index) const {
174  if (index >= 0) {
175  return at(index);
176  } else {
177  return at((int)size() + index);
178  }
179 }
180 
181 
182 Position&
184  if (index >= 0) {
185  return at(index);
186  } else {
187  return at((int)size() + index);
188  }
189 }
190 
191 
192 Position
194  const_iterator i = begin();
195  SUMOReal seenLength = 0;
196  do {
197  const SUMOReal nextLength = (*i).distanceTo(*(i + 1));
198  if (seenLength + nextLength > pos) {
199  return positionAtOffset(*i, *(i + 1), pos - seenLength, lateralOffset);
200  }
201  seenLength += nextLength;
202  } while (++i != end() - 1);
203  return back();
204 }
205 
206 
207 Position
209  const_iterator i = begin();
210  SUMOReal seenLength = 0;
211  do {
212  const SUMOReal nextLength = (*i).distanceTo2D(*(i + 1));
213  if (seenLength + nextLength > pos) {
214  return positionAtOffset2D(*i, *(i + 1), pos - seenLength, lateralOffset);
215  }
216  seenLength += nextLength;
217  } while (++i != end() - 1);
218  return back();
219 }
220 
221 
222 SUMOReal
224  if (pos < 0) {
225  pos += length();
226  }
227  const_iterator i = begin();
228  SUMOReal seenLength = 0;
229  do {
230  const Position& p1 = *i;
231  const Position& p2 = *(i + 1);
232  const SUMOReal nextLength = p1.distanceTo(p2);
233  if (seenLength + nextLength > pos) {
234  return p1.angleTo2D(p2);
235  }
236  seenLength += nextLength;
237  } while (++i != end() - 1);
238  const Position& p1 = (*this)[-2];
239  const Position& p2 = back();
240  return p1.angleTo2D(p2);
241 }
242 
243 
244 SUMOReal
247 }
248 
249 
250 SUMOReal
252  const_iterator i = begin();
253  SUMOReal seenLength = 0;
254  do {
255  const Position& p1 = *i;
256  const Position& p2 = *(i + 1);
257  const SUMOReal nextLength = p1.distanceTo(p2);
258  if (seenLength + nextLength > pos) {
259  return RAD2DEG(atan2(p2.z() - p1.z(), p1.distanceTo2D(p2)));
260  }
261  seenLength += nextLength;
262  } while (++i != end() - 1);
263  const Position& p1 = (*this)[-2];
264  const Position& p2 = back();
265  return RAD2DEG(atan2(p2.z() - p1.z(), p1.distanceTo2D(p2)));
266 }
267 
268 Position
270  const Position& p2,
271  SUMOReal pos, SUMOReal lateralOffset) {
272  const SUMOReal dist = p1.distanceTo(p2);
273  if (pos < 0 || dist < pos) {
274  return Position::INVALID;
275  }
276  if (lateralOffset != 0) {
277  const Position offset = sideOffset(p1, p2, -lateralOffset); // move in the same direction as Position::move2side
278  if (pos == 0.) {
279  return p1 + offset;
280  }
281  return p1 + (p2 - p1) * (pos / dist) + offset;
282  }
283  if (pos == 0.) {
284  return p1;
285  }
286  return p1 + (p2 - p1) * (pos / dist);
287 }
288 
289 
290 Position
292  const Position& p2,
293  SUMOReal pos, SUMOReal lateralOffset) {
294  const SUMOReal dist = p1.distanceTo2D(p2);
295  if (pos < 0 || dist < pos) {
296  return Position::INVALID;
297  }
298  if (lateralOffset != 0) {
299  const Position offset = sideOffset(p1, p2, -lateralOffset); // move in the same direction as Position::move2side
300  if (pos == 0.) {
301  return p1 + offset;
302  }
303  return p1 + (p2 - p1) * (pos / dist) + offset;
304  }
305  if (pos == 0.) {
306  return p1;
307  }
308  return p1 + (p2 - p1) * (pos / dist);
309 }
310 
311 
312 Boundary
314  Boundary ret;
315  for (const_iterator i = begin(); i != end(); i++) {
316  ret.add(*i);
317  }
318  return ret;
319 }
320 
321 
322 Position
324  SUMOReal x = 0;
325  SUMOReal y = 0;
326  SUMOReal z = 0;
327  for (const_iterator i = begin(); i != end(); i++) {
328  x += (*i).x();
329  y += (*i).y();
330  z += (*i).z();
331  }
332  return Position(x / (SUMOReal) size(), y / (SUMOReal) size(), z / (SUMOReal)size());
333 }
334 
335 
336 Position
338  PositionVector tmp = *this;
339  if (!isClosed()) { // make sure its closed
340  tmp.push_back(tmp[0]);
341  }
342  const int endIndex = (int)tmp.size() - 1;
343  SUMOReal div = 0; // 6 * area including sign
344  SUMOReal x = 0;
345  SUMOReal y = 0;
346  if (tmp.area() != 0) { // numerical instability ?
347  // http://en.wikipedia.org/wiki/Polygon
348  for (int i = 0; i < endIndex; i++) {
349  const SUMOReal z = tmp[i].x() * tmp[i + 1].y() - tmp[i + 1].x() * tmp[i].y();
350  div += z; // area formula
351  x += (tmp[i].x() + tmp[i + 1].x()) * z;
352  y += (tmp[i].y() + tmp[i + 1].y()) * z;
353  }
354  div *= 3; // 6 / 2, the 2 comes from the area formula
355  return Position(x / div, y / div);
356  } else {
357  // compute by decomposing into line segments
358  // http://en.wikipedia.org/wiki/Centroid#By_geometric_decomposition
359  SUMOReal lengthSum = 0;
360  for (int i = 0; i < endIndex; i++) {
361  SUMOReal length = tmp[i].distanceTo(tmp[i + 1]);
362  x += (tmp[i].x() + tmp[i + 1].x()) * length / 2;
363  y += (tmp[i].y() + tmp[i + 1].y()) * length / 2;
364  lengthSum += length;
365  }
366  if (lengthSum == 0) {
367  // it is probably only one point
368  return tmp[0];
369  }
370  return Position(x / lengthSum, y / lengthSum);
371  }
372 }
373 
374 
375 void
377  Position centroid = getCentroid();
378  for (int i = 0; i < static_cast<int>(size()); i++) {
379  (*this)[i] = centroid + (((*this)[i] - centroid) * factor);
380  }
381 }
382 
383 
384 void
386  Position centroid = getCentroid();
387  for (int i = 0; i < static_cast<int>(size()); i++) {
388  (*this)[i] = centroid + (((*this)[i] - centroid) + offset);
389  }
390 }
391 
392 
393 Position
395  if (size() == 1) {
396  return (*this)[0];
397  }
398  return positionAtOffset(SUMOReal((length() / 2.)));
399 }
400 
401 
402 SUMOReal
404  SUMOReal len = 0;
405  for (const_iterator i = begin(); i != end() - 1; i++) {
406  len += (*i).distanceTo(*(i + 1));
407  }
408  return len;
409 }
410 
411 SUMOReal
413  SUMOReal len = 0;
414  for (const_iterator i = begin(); i != end() - 1; i++) {
415  len += (*i).distanceTo2D(*(i + 1));
416  }
417  return len;
418 }
419 
420 
421 SUMOReal
423  if (size() < 3) {
424  return 0;
425  }
426  SUMOReal area = 0;
427  PositionVector tmp = *this;
428  if (!isClosed()) { // make sure its closed
429  tmp.push_back(tmp[0]);
430  }
431  const int endIndex = (int)tmp.size() - 1;
432  // http://en.wikipedia.org/wiki/Polygon
433  for (int i = 0; i < endIndex; i++) {
434  area += tmp[i].x() * tmp[i + 1].y() - tmp[i + 1].x() * tmp[i].y();
435  }
436  if (area < 0) { // we whether we had cw or ccw order
437  area *= -1;
438  }
439  return area / 2;
440 }
441 
442 
443 bool
445  for (const_iterator i = begin(); i != end() - 1; i++) {
446  if (poly.around(*i, offset)) {
447  return true;
448  }
449  }
450  return false;
451 }
452 
453 
454 bool
455 PositionVector::crosses(const Position& p1, const Position& p2) const {
456  return intersects(p1, p2);
457 }
458 
459 
460 std::pair<PositionVector, PositionVector>
462  if (size() < 2) {
463  throw InvalidArgument("Vector to short for splitting");
464  }
465  if (where <= POSITION_EPS || where >= length() - POSITION_EPS) {
466  WRITE_WARNING("Splitting vector close to end (pos: " + toString(where) + ", length: " + toString(length()) + ")");
467  }
468  PositionVector first, second;
469  first.push_back((*this)[0]);
470  SUMOReal seen = 0;
471  const_iterator it = begin() + 1;
472  SUMOReal next = first.back().distanceTo(*it);
473  // see how many points we can add to first
474  while (where >= seen + next + POSITION_EPS) {
475  seen += next;
476  first.push_back(*it);
477  it++;
478  next = first.back().distanceTo(*it);
479  }
480  if (fabs(where - (seen + next)) > POSITION_EPS || it == end() - 1) {
481  // we need to insert a new point because 'where' is not close to an
482  // existing point or it is to close to the endpoint
483  const Position p = positionAtOffset(first.back(), *it, where - seen);
484  first.push_back(p);
485  second.push_back(p);
486  } else {
487  first.push_back(*it);
488  }
489  // add the remaining points to second
490  for (; it != end(); it++) {
491  second.push_back(*it);
492  }
493  assert(first.size() >= 2);
494  assert(second.size() >= 2);
495  assert(first.back() == second.front());
496  assert(fabs(first.length() + second.length() - length()) < 2 * POSITION_EPS);
497  return std::pair<PositionVector, PositionVector>(first, second);
498 }
499 
500 
501 std::ostream&
502 operator<<(std::ostream& os, const PositionVector& geom) {
503  for (PositionVector::const_iterator i = geom.begin(); i != geom.end(); i++) {
504  if (i != geom.begin()) {
505  os << " ";
506  }
507  os << (*i);
508  }
509  return os;
510 }
511 
512 
513 void
515  std::sort(begin(), end(), as_poly_cw_sorter());
516 }
517 
518 
519 void
521  for (int i = 0; i < static_cast<int>(size()); i++) {
522  (*this)[i].add(xoff, yoff, zoff);
523  }
524 }
525 
526 
527 void
529  for (int i = 0; i < static_cast<int>(size()); i++) {
530  (*this)[i].mul(1, -1);
531  }
532 }
533 
534 
535 int
537  const Position& p2) const {
538  return atan2(p1.x(), p1.y()) < atan2(p2.x(), p2.y());
539 }
540 
541 
542 
543 void
545  std::sort(begin(), end(), increasing_x_y_sorter());
546 }
547 
548 
549 
551 
552 
553 
554 int
556  const Position& p2) const {
557  if (p1.x() != p2.x()) {
558  return p1.x() < p2.x();
559  }
560  return p1.y() < p2.y();
561 }
562 
563 
564 
565 SUMOReal
567  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  // XXX 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 * distance(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 std::vector<SUMOReal>
803  std::vector<SUMOReal> ret;
804  for (const_iterator i = other.begin(); i != other.end() - 1; i++) {
805  std::vector<SUMOReal> atSegment = intersectsAtLengths2D(*i, *(i + 1));
806  copy(atSegment.begin(), atSegment.end(), back_inserter(ret));
807  }
808  return ret;
809 }
810 
811 
812 std::vector<SUMOReal>
814  std::vector<SUMOReal> ret;
815  SUMOReal pos = 0;
816  for (const_iterator i = begin(); i != end() - 1; i++) {
817  const Position& p1 = *i;
818  const Position& p2 = *(i + 1);
819  SUMOReal x, y, m;
820  if (intersects(p1, p2, lp1, lp2, 0., &x, &y, &m)) {
821  ret.push_back(Position(x, y).distanceTo2D(p1) + pos);
822  }
823  pos += p1.distanceTo2D(p2);
824  }
825  return ret;
826 }
827 
828 
829 void
830 PositionVector::extrapolate(const SUMOReal val, const bool onlyFirst) {
831  assert(size() > 1);
832  Position& p1 = (*this)[0];
833  Position& p2 = (*this)[1];
834  const Position offset = (p2 - p1) * (val / p1.distanceTo(p2));
835  p1.sub(offset);
836  if (!onlyFirst) {
837  if (size() == 2) {
838  p2.add(offset);
839  } else {
840  const Position& e1 = (*this)[-2];
841  Position& e2 = (*this)[-1];
842  e2.sub((e1 - e2) * (val / e1.distanceTo(e2)));
843  }
844  }
845 }
846 
847 
848 void
849 PositionVector::extrapolate2D(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.distanceTo2D(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.distanceTo2D(e2)));
862  }
863  }
864 }
865 
866 
869  PositionVector ret;
870  for (const_reverse_iterator i = rbegin(); i != rend(); i++) {
871  ret.push_back(*i);
872  }
873  return ret;
874 }
875 
876 
877 Position
878 PositionVector::sideOffset(const Position& beg, const Position& end, const SUMOReal amount) {
879  const SUMOReal scale = amount / beg.distanceTo2D(end);
880  return Position((beg.y() - end.y()) * scale, (end.x() - beg.x()) * scale);
881 }
882 
883 
884 void
886  if (size() < 2) {
887  return;
888  }
889  PositionVector shape;
890  for (int i = 0; i < static_cast<int>(size()); i++) {
891  if (i == 0) {
892  const Position& from = (*this)[i];
893  const Position& to = (*this)[i + 1];
894  shape.push_back(from - sideOffset(from, to, amount));
895  } else if (i == static_cast<int>(size()) - 1) {
896  const Position& from = (*this)[i - 1];
897  const Position& to = (*this)[i];
898  shape.push_back(to - sideOffset(from, to, amount));
899  } else {
900  const Position& from = (*this)[i - 1];
901  const Position& me = (*this)[i];
902  const Position& to = (*this)[i + 1];
903  PositionVector fromMe(from, me);
904  fromMe.extrapolate2D(me.distanceTo2D(to));
905  const SUMOReal extrapolateDev = fromMe[1].distanceTo2D(to);
906  if (fabs(extrapolateDev) < POSITION_EPS) {
907  // parallel case, just shift the middle point
908  shape.push_back(me - sideOffset(from, to, amount));
909  } else if (fabs(extrapolateDev - 2 * me.distanceTo2D(to)) < POSITION_EPS) {
910  // counterparallel case, just shift the middle point
911  PositionVector fromMe(from, me);
912  fromMe.extrapolate2D(amount);
913  shape.push_back(fromMe[1]);
914  } else {
915  Position offsets = sideOffset(from, me, amount);
916  Position offsets2 = sideOffset(me, to, amount);
917  PositionVector l1(from - offsets, me - offsets);
918  PositionVector l2(me - offsets2, to - offsets2);
919  shape.push_back(l1.intersectionPosition2D(l2[0], l2[1], 100));
920  if (shape.back() == Position::INVALID) {
921  throw InvalidArgument("no line intersection");
922  }
923  }
924  // copy original z value
925  shape.back().set(shape.back().x(), shape.back().y(), me.z());
926  }
927  }
928  *this = shape;
929 }
930 
931 
932 SUMOReal
934  assert((int)size() > pos + 1);
935  return (*this)[pos].angleTo2D((*this)[pos + 1]);
936 }
937 
938 
939 void
941  if (size() == 0 || (*this)[0] == back()) {
942  return;
943  }
944  push_back((*this)[0]);
945 }
946 
947 
948 std::vector<SUMOReal>
949 PositionVector::distances(const PositionVector& s, bool perpendicular) const {
950  std::vector<SUMOReal> ret;
951  const_iterator i;
952  for (i = begin(); i != end(); i++) {
953  const SUMOReal dist = s.distance(*i, perpendicular);
954  if (dist != GeomHelper::INVALID_OFFSET) {
955  ret.push_back(dist);
956  }
957  }
958  for (i = s.begin(); i != s.end(); i++) {
959  const SUMOReal dist = distance(*i, perpendicular);
960  if (dist != GeomHelper::INVALID_OFFSET) {
961  ret.push_back(dist);
962  }
963  }
964  return ret;
965 }
966 
967 
968 SUMOReal
969 PositionVector::distance(const Position& p, bool perpendicular) const {
970  if (size() == 0) {
972  } else if (size() == 1) {
973  return front().distanceTo(p);
974  }
975  const SUMOReal nearestOffset = nearest_offset_to_point2D(p, perpendicular);
976  if (nearestOffset == GeomHelper::INVALID_OFFSET) {
978  } else {
979  return p.distanceTo2D(positionAtOffset2D(nearestOffset));
980  }
981 }
982 
983 void
985  if (size() == 0 || !p.almostSame(back())) {
986  push_back(p);
987  }
988 }
989 
990 
991 void
993  if (size() == 0 || !p.almostSame(front())) {
994  insert(begin(), p);
995  }
996 }
997 
998 
999 bool
1001  return size() >= 2 && (*this)[0] == back();
1002 }
1003 
1004 
1005 void
1006 PositionVector::removeDoublePoints(SUMOReal minDist, bool assertLength) {
1007  if (size() > 1) {
1008  iterator last = begin();
1009  for (iterator i = begin() + 1; i != end() && (!assertLength || size() > 2);) {
1010  if (last->almostSame(*i, minDist)) {
1011  i = erase(i);
1012  } else {
1013  last = i;
1014  ++i;
1015  }
1016  }
1017  }
1018 }
1019 
1020 
1021 bool
1023  if (size() == v2.size()) {
1024  for (int i = 0; i < (int)size(); i++) {
1025  if ((*this)[i] != v2[i]) {
1026  return false;
1027  }
1028  }
1029  return true;
1030  } else {
1031  return false;
1032  }
1033 }
1034 
1035 
1036 bool
1038  if (size() > 2) {
1039  return false;
1040  }
1041  for (const_iterator i = begin(); i != end() - 1; i++) {
1042  if ((*i).z() != (*(i + 1)).z()) {
1043  return true;
1044  }
1045  }
1046  return false;
1047 }
1048 
1049 
1050 bool
1052  const Position& p21, const Position& p22,
1053  const SUMOReal withinDist,
1054  SUMOReal* x, SUMOReal* y, SUMOReal* mu) {
1055  const SUMOReal eps = std::numeric_limits<SUMOReal>::epsilon();
1056  const double denominator = (p22.y() - p21.y()) * (p12.x() - p11.x()) - (p22.x() - p21.x()) * (p12.y() - p11.y());
1057  const double numera = (p22.x() - p21.x()) * (p11.y() - p21.y()) - (p22.y() - p21.y()) * (p11.x() - p21.x());
1058  const double numerb = (p12.x() - p11.x()) * (p11.y() - p21.y()) - (p12.y() - p11.y()) * (p11.x() - p21.x());
1059  /* Are the lines coincident? */
1060  if (fabs(numera) < eps && fabs(numerb) < eps && fabs(denominator) < eps) {
1061  SUMOReal a1;
1062  SUMOReal a2;
1063  SUMOReal a3;
1064  SUMOReal a4;
1065  SUMOReal a = -1e12;
1066  if (p11.x() != p12.x()) {
1067  a1 = p11.x() < p12.x() ? p11.x() : p12.x();
1068  a2 = p11.x() < p12.x() ? p12.x() : p11.x();
1069  a3 = p21.x() < p22.x() ? p21.x() : p22.x();
1070  a4 = p21.x() < p22.x() ? p22.x() : p21.x();
1071  } else {
1072  a1 = p11.y() < p12.y() ? p11.y() : p12.y();
1073  a2 = p11.y() < p12.y() ? p12.y() : p11.y();
1074  a3 = p21.y() < p22.y() ? p21.y() : p22.y();
1075  a4 = p21.y() < p22.y() ? p22.y() : p21.y();
1076  }
1077  if (a1 <= a3 && a3 <= a2) {
1078  if (a4 < a2) {
1079  a = (a3 + a4) / 2;
1080  } else {
1081  a = (a2 + a3) / 2;
1082  }
1083  }
1084  if (a3 <= a1 && a1 <= a4) {
1085  if (a2 < a4) {
1086  a = (a1 + a2) / 2;
1087  } else {
1088  a = (a1 + a4) / 2;
1089  }
1090  }
1091  if (a != -1e12) {
1092  if (x != 0) {
1093  if (p11.x() != p12.x()) {
1094  *mu = (a - p11.x()) / (p12.x() - p11.x());
1095  *x = a;
1096  *y = p11.y() + (*mu) * (p12.y() - p11.y());
1097  } else {
1098  *x = p11.x();
1099  *y = a;
1100  if (p12.y() == p11.y()) {
1101  *mu = 0;
1102  } else {
1103  *mu = (a - p11.y()) / (p12.y() - p11.y());
1104  }
1105  }
1106  }
1107  return true;
1108  }
1109  return false;
1110  }
1111  /* Are the lines parallel */
1112  if (fabs(denominator) < eps) {
1113  return false;
1114  }
1115  /* Is the intersection along the segments */
1116  double mua = numera / denominator;
1117  /* reduce rounding errors for lines ending in the same point */
1118  if (fabs(p12.x() - p22.x()) < eps && fabs(p12.y() - p22.y()) < eps) {
1119  mua = 1.;
1120  } else {
1121  const double offseta = withinDist / p11.distanceTo2D(p12);
1122  const double offsetb = withinDist / p21.distanceTo2D(p22);
1123  const double mub = numerb / denominator;
1124  if (mua < -offseta || mua > 1 + offseta || mub < -offsetb || mub > 1 + offsetb) {
1125  return false;
1126  }
1127  }
1128  if (x != 0) {
1129  *x = p11.x() + mua * (p12.x() - p11.x());
1130  *y = p11.y() + mua * (p12.y() - p11.y());
1131  *mu = mua;
1132  }
1133  return true;
1134 }
1135 
1136 
1137 /****************************************************************************/
1138 
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
static Position sideOffset(const Position &beg, const Position &end, const SUMOReal amount)
bool hasElevation() const
return whether two positions differ in z-coordinate
SUMOReal distance(const Position &p, bool perpendicular=false) const
SUMOReal rotationAtOffset(SUMOReal pos) const
Returns the rotation at the given length.
SUMOReal nearest_offset_to_point2D(const Position &p, bool perpendicular=true) const
PositionVector getSubpart2D(SUMOReal beginOffset, SUMOReal endOffset) const
void sortAsPolyCWByAngle()
void add(const Position &pos)
Adds the given position to this one.
Definition: Position.h:119
#define M_PI
Definition: angles.h:37
friend std::ostream & operator<<(std::ostream &os, const PositionVector &geom)
Output operator.
Position getCentroid() const
Returns the centroid (closes the polygon if unclosed)
bool intersects(const Position &p1, const Position &p2) const
void scaleRelative(SUMOReal factor)
enlarges/shrinks the polygon by a factor based at the centroid
PositionVector getSubpartByIndex(int beginIndex, int count) const
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
const Position & operator[](int index) const
returns the 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
PositionVector reverse() const
SUMOReal slopeDegreeAtOffset(SUMOReal pos) const
Returns the slope at the given length.
PositionVector convexHull() const
~PositionVector()
Destructor.
void extrapolate2D(const SUMOReal val, const bool onlyFirst=false)
SUMOReal length2D() const
Returns the length.
std::vector< SUMOReal > distances(const PositionVector &s, bool perpendicular=false) const
void push_front_noDoublePos(const Position &p)
#define max(a, b)
Definition: polyfonts.c:65
static SUMOReal legacyDegree(const SUMOReal angle, const bool positive=false)
Definition: GeomHelper.cpp:204
void mirrorX()
mirror coordinates along the x-axis
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
int operator()(const Position &p1, const Position &p2) const
comparing operation
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:187
int insertAtClosest(const Position &p)
std::string toString(const T &t, std::streamsize accuracy=OUTPUT_ACCURACY)
Definition: ToString.h:54
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.
SUMOReal length() const
Returns the length.
SUMOReal rotationDegreeAtOffset(SUMOReal pos) const
Returns the rotation at the given length.
void add(SUMOReal x, SUMOReal y)
Makes the boundary include the given coordinate.
Definition: Boundary.cpp:76
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
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 Again a boundary may be specifie...
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
Position getLineCenter() const
SUMOReal distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
Definition: Position.h:232
void move2side(SUMOReal amount)
#define SUMOReal
Definition: config.h:213
void push_back_noDoublePos(const Position &p)
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
PositionVector getSubpart(SUMOReal beginOffset, SUMOReal endOffset) const
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)
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
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...