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-2015 DLR (http://www.dlr.de/) and contributors
14 /****************************************************************************/
15 //
16 // This file is part of SUMO.
17 // SUMO is free software: you can redistribute it and/or modify
18 // it under the terms of the GNU General Public License as published by
19 // the Free Software Foundation, either version 3 of the License, or
20 // (at your option) any later version.
21 //
22 /****************************************************************************/
23 
24 
25 // ===========================================================================
26 // included modules
27 // ===========================================================================
28 #ifdef _MSC_VER
29 #include <windows_config.h>
30 #else
31 #include <config.h>
32 #endif
33 
34 #include <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 "Line.h"
49 #include "Helper_ConvexHull.h"
50 #include "Boundary.h"
51 
52 #ifdef CHECK_MEMORY_LEAKS
53 #include <foreign/nvwa/debug_new.h>
54 #endif // CHECK_MEMORY_LEAKS
55 
56 // ===========================================================================
57 // method definitions
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 
68 
69 
70 // ------------ Adding items to the container
71 void
73  copy(p.begin(), p.end(), back_inserter(*this));
74 }
75 
76 
77 void
79  insert(begin(), p);
80 }
81 
82 
85  Position first = front();
86  erase(begin());
87  return first;
88 }
89 
90 
91 bool
92 PositionVector::around(const Position& p, SUMOReal offset) const {
93  if (offset != 0) {
94  PositionVector tmp(*this);
95  tmp.scaleAbsolute(offset);
96  return tmp.around(p);
97  }
98  SUMOReal angle = 0;
99  for (const_iterator i = begin(); i != end() - 1; i++) {
100  Position p1(
101  (*i).x() - p.x(),
102  (*i).y() - p.y());
103  Position p2(
104  (*(i + 1)).x() - p.x(),
105  (*(i + 1)).y() - p.y());
106  angle += GeomHelper::Angle2D(p1.x(), p1.y(), p2.x(), p2.y());
107  }
108  Position p1(
109  (*(end() - 1)).x() - p.x(),
110  (*(end() - 1)).y() - p.y());
111  Position p2(
112  (*(begin())).x() - p.x(),
113  (*(begin())).y() - p.y());
114  angle += GeomHelper::Angle2D(p1.x(), p1.y(), p2.x(), p2.y());
115  return (!(fabs(angle) < M_PI));
116 }
117 
118 
119 bool
121  for (const_iterator i = begin(); i != end() - 1; i++) {
122  if (poly.around(*i, offset)) {
123  return true;
124  }
125  }
126  return false;
127 }
128 
129 
130 bool
131 PositionVector::intersects(const Position& p1, const Position& p2) const {
132  if (size() < 2) {
133  return false;
134  }
135  for (const_iterator i = begin(); i != end() - 1; i++) {
136  if (GeomHelper::intersects(*i, *(i + 1), p1, p2)) {
137  return true;
138  }
139  }
140  //return GeomHelper::intersects(*(end()-1), *(begin()), p1, p2);
141  return false;
142 }
143 
144 
145 bool
147  if (size() < 2) {
148  return false;
149  }
150  for (const_iterator i = begin(); i != end() - 1; i++) {
151  if (v1.intersects(*i, *(i + 1))) {
152  return true;
153  }
154  }
155  //return v1.intersects(*(end()-1), *(begin()));
156  return false;
157 }
158 
159 
160 Position
162  const Position& p2) const {
163  for (const_iterator i = begin(); i != end() - 1; i++) {
164  if (GeomHelper::intersects(*i, *(i + 1), p1, p2)) {
165  return GeomHelper::intersection_position2D(*i, *(i + 1), p1, p2);
166  }
167  }
168  return Position::INVALID;
169 }
170 
171 
172 Position
174  for (const_iterator i = begin(); i != end() - 1; i++) {
175  if (v1.intersects(*i, *(i + 1))) {
176  return v1.intersectsAtPoint(*i, *(i + 1));
177  }
178  }
179  /*
180  if(v1.intersects(*(end()-1), *(begin()))) {
181  return v1.intersectsAtPoint(*(end()-1), *(begin()));
182  }
183  */
184  return Position::INVALID;
185 }
186 
187 
188 const Position&
189 PositionVector::operator[](int index) const {
190  if (index >= 0) {
191  return at(index);
192  } else {
193  return at((int)size() + index);
194  }
195 }
196 
197 
198 Position&
200  if (index >= 0) {
201  return at(index);
202  } else {
203  return at((int)size() + index);
204  }
205 }
206 
207 
208 Position
210  const_iterator i = begin();
211  SUMOReal seenLength = 0;
212  do {
213  const SUMOReal nextLength = (*i).distanceTo(*(i + 1));
214  if (seenLength + nextLength > pos) {
215  return positionAtOffset(*i, *(i + 1), pos - seenLength, lateralOffset);
216  }
217  seenLength += nextLength;
218  } while (++i != end() - 1);
219  return back();
220 }
221 
222 
223 Position
225  const_iterator i = begin();
226  SUMOReal seenLength = 0;
227  do {
228  const SUMOReal nextLength = (*i).distanceTo2D(*(i + 1));
229  if (seenLength + nextLength > pos) {
230  return positionAtOffset2D(*i, *(i + 1), pos - seenLength, lateralOffset);
231  }
232  seenLength += nextLength;
233  } while (++i != end() - 1);
234  return back();
235 }
236 
237 
238 SUMOReal
240  if (pos < 0) {
241  pos += length();
242  }
243  const_iterator i = begin();
244  SUMOReal seenLength = 0;
245  do {
246  SUMOReal nextLength = (*i).distanceTo(*(i + 1));
247  if (seenLength + nextLength > pos) {
248  Line l(*i, *(i + 1));
249  return l.atan2DegreeAngle();
250  }
251  seenLength += nextLength;
252  } while (++i != end() - 1);
253  Line l(*(end() - 2), *(end() - 1));
254  return l.atan2DegreeAngle();
255 }
256 
257 SUMOReal
259  const_iterator i = begin();
260  SUMOReal seenLength = 0;
261  do {
262  SUMOReal nextLength = (*i).distanceTo(*(i + 1));
263  if (seenLength + nextLength > pos) {
264  Line l(*i, *(i + 1));
265  return l.atan2DegreeSlope();
266  }
267  seenLength += nextLength;
268  } while (++i != end() - 1);
269  Line l(*(end() - 2), *(end() - 1));
270  return l.atan2DegreeSlope();
271 }
272 
273 Position
275  const Position& p2,
276  SUMOReal pos, SUMOReal lateralOffset) {
277  const SUMOReal dist = p1.distanceTo(p2);
278  if (dist < pos) {
279  return Position::INVALID;
280  }
281  if (lateralOffset != 0) {
282  Line l(p1, p2);
283  l.move2side(-lateralOffset); // move in the same direction as Position::move2side
284  return l.getPositionAtDistance(pos);
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 (dist < pos) {
296  return Position::INVALID;
297  }
298  if (lateralOffset != 0) {
299  Line l(p1, p2);
300  l.move2side(-lateralOffset); // move in the same direction as Position::move2side
301  return l.getPositionAtDistance2D(pos);
302  }
303  return p1 + (p2 - p1) * (pos / dist);
304 }
305 
306 
307 Boundary
309  Boundary ret;
310  for (const_iterator i = begin(); i != end(); i++) {
311  ret.add(*i);
312  }
313  return ret;
314 }
315 
316 
317 Position
319  SUMOReal x = 0;
320  SUMOReal y = 0;
321  for (const_iterator i = begin(); i != end(); i++) {
322  x += (*i).x();
323  y += (*i).y();
324  }
325  return Position(x / (SUMOReal) size(), y / (SUMOReal) size());
326 }
327 
328 
329 Position
331  PositionVector tmp = *this;
332  if (!isClosed()) { // make sure its closed
333  tmp.push_back(tmp[0]);
334  }
335  const int endIndex = (int)tmp.size() - 1;
336  SUMOReal div = 0; // 6 * area including sign
337  SUMOReal x = 0;
338  SUMOReal y = 0;
339  if (tmp.area() != 0) { // numerical instability ?
340  // http://en.wikipedia.org/wiki/Polygon
341  for (int i = 0; i < endIndex; i++) {
342  const SUMOReal z = tmp[i].x() * tmp[i + 1].y() - tmp[i + 1].x() * tmp[i].y();
343  div += z; // area formula
344  x += (tmp[i].x() + tmp[i + 1].x()) * z;
345  y += (tmp[i].y() + tmp[i + 1].y()) * z;
346  }
347  div *= 3; // 6 / 2, the 2 comes from the area formula
348  return Position(x / div, y / div);
349  } else {
350  // compute by decomposing into line segments
351  // http://en.wikipedia.org/wiki/Centroid#By_geometric_decomposition
352  SUMOReal lengthSum = 0;
353  for (int i = 0; i < endIndex; i++) {
354  SUMOReal length = tmp[i].distanceTo(tmp[i + 1]);
355  x += (tmp[i].x() + tmp[i + 1].x()) * length / 2;
356  y += (tmp[i].y() + tmp[i + 1].y()) * length / 2;
357  lengthSum += length;
358  }
359  if (lengthSum == 0) {
360  // it is probably only one point
361  return tmp[0];
362  }
363  return Position(x / lengthSum, y / lengthSum);
364  }
365 }
366 
367 
368 void
370  Position centroid = getCentroid();
371  for (int i = 0; i < static_cast<int>(size()); i++) {
372  (*this)[i] = centroid + (((*this)[i] - centroid) * factor);
373  }
374 }
375 
376 
377 void
379  Position centroid = getCentroid();
380  for (int i = 0; i < static_cast<int>(size()); i++) {
381  (*this)[i] = centroid + (((*this)[i] - centroid) + offset);
382  }
383 }
384 
385 
386 Position
388  if (size() == 1) {
389  return (*this)[0];
390  }
391  return positionAtOffset(SUMOReal((length() / 2.)));
392 }
393 
394 
395 SUMOReal
397  SUMOReal len = 0;
398  for (const_iterator i = begin(); i != end() - 1; i++) {
399  len += (*i).distanceTo(*(i + 1));
400  }
401  return len;
402 }
403 
404 SUMOReal
406  SUMOReal len = 0;
407  for (const_iterator i = begin(); i != end() - 1; i++) {
408  len += (*i).distanceTo2D(*(i + 1));
409  }
410  return len;
411 }
412 
413 
414 SUMOReal
416  if (size() < 3) {
417  return 0;
418  }
419  SUMOReal area = 0;
420  PositionVector tmp = *this;
421  if (!isClosed()) { // make sure its closed
422  tmp.push_back(tmp[0]);
423  }
424  const int endIndex = (int)tmp.size() - 1;
425  // http://en.wikipedia.org/wiki/Polygon
426  for (int i = 0; i < endIndex; i++) {
427  area += tmp[i].x() * tmp[i + 1].y() - tmp[i + 1].x() * tmp[i].y();
428  }
429  if (area < 0) { // we whether we had cw or ccw order
430  area *= -1;
431  }
432  return area / 2;
433 }
434 
435 
436 bool
438  for (const_iterator i = begin(); i != end() - 1; i++) {
439  if (poly.around(*i, offset)) {
440  return true;
441  }
442  }
443  return false;
444 }
445 
446 
447 bool
448 PositionVector::crosses(const Position& p1, const Position& p2) const {
449  return intersects(p1, p2);
450 }
451 
452 
453 std::pair<PositionVector, PositionVector>
455  if (size() < 2) {
456  throw InvalidArgument("Vector to short for splitting");
457  }
458  if (where <= POSITION_EPS || where >= length() - POSITION_EPS) {
459  WRITE_WARNING("Splitting vector close to end (pos: " + toString(where) + ", length: " + toString(length()) + ")");
460  }
461  PositionVector first, second;
462  first.push_back((*this)[0]);
463  SUMOReal seen = 0;
464  const_iterator it = begin() + 1;
465  SUMOReal next = first.back().distanceTo(*it);
466  // see how many points we can add to first
467  while (where >= seen + next + POSITION_EPS) {
468  seen += next;
469  first.push_back(*it);
470  it++;
471  next = first.back().distanceTo(*it);
472  }
473  if (fabs(where - (seen + next)) > POSITION_EPS || it == end() - 1) {
474  // we need to insert a new point because 'where' is not close to an
475  // existing point or it is to close to the endpoint
476  Line tmpL(first.back(), *it);
477  Position p = tmpL.getPositionAtDistance(where - seen);
478  first.push_back(p);
479  second.push_back(p);
480  } else {
481  first.push_back(*it);
482  }
483  // add the remaining points to second
484  for (; it != end(); it++) {
485  second.push_back(*it);
486  }
487  assert(first.size() >= 2);
488  assert(second.size() >= 2);
489  assert(first.back() == second.front());
490  assert(fabs(first.length() + second.length() - length()) < 2 * POSITION_EPS);
491  return std::pair<PositionVector, PositionVector>(first, second);
492 }
493 
494 
495 std::ostream&
496 operator<<(std::ostream& os, const PositionVector& geom) {
497  for (PositionVector::const_iterator i = geom.begin(); i != geom.end(); i++) {
498  if (i != geom.begin()) {
499  os << " ";
500  }
501  os << (*i);
502  }
503  return os;
504 }
505 
506 
507 void
509  std::sort(begin(), end(), as_poly_cw_sorter());
510 }
511 
512 
513 void
515  for (int i = 0; i < static_cast<int>(size()); i++) {
516  (*this)[i].add(xoff, yoff, zoff);
517  }
518 }
519 
520 
521 void
523  for (int i = 0; i < static_cast<int>(size()); i++) {
524  (*this)[i].reshiftRotate(xoff, yoff, rot);
525  }
526 }
527 
528 
529 int
531  const Position& p2) const {
532  return atan2(p1.x(), p1.y()) < atan2(p2.x(), p2.y());
533 }
534 
535 
536 
537 void
539  std::sort(begin(), end(), increasing_x_y_sorter());
540 }
541 
542 
543 
545 
546 
547 
548 int
550  const Position& p2) const {
551  if (p1.x() != p2.x()) {
552  return p1.x() < p2.x();
553  }
554  return p1.y() < p2.y();
555 }
556 
557 
558 
559 SUMOReal
561  const Position& P2) const {
562  return (P1.x() - P0.x()) * (P2.y() - P0.y()) - (P2.x() - P0.x()) * (P1.y() - P0.y());
563 }
564 
565 
568  PositionVector ret = *this;
569  ret.sortAsPolyCWByAngle();
570  return simpleHull_2D(ret);
571 }
572 
573 
576  PositionVector ret;
577  for (const_iterator i = begin(); i != end() - 1; i++) {
578  if (GeomHelper::intersects(*i, *(i + 1), line.p1(), line.p2())) {
580  *i, *(i + 1), line.p1(), line.p2()));
581  }
582  }
583  return ret;
584 }
585 
586 
587 int
589  if (back().distanceTo(v[0]) < 2) { // !!! heuristic
590  copy(v.begin() + 1, v.end(), back_inserter(*this));
591  return 1;
592  }
593  //
594  Line l1((*this)[static_cast<int>(size()) - 2], back());
595  l1.extrapolateBy(100);
596  Line l2(v[0], v[1]);
597  l2.extrapolateBy(100);
598  if (l1.intersects(l2) && l1.intersectsAtLength2D(l2) < l1.length2D() - 100) { // !!! heuristic
599  Position p = l1.intersectsAt(l2);
600  (*this)[static_cast<int>(size()) - 1] = p;
601  copy(v.begin() + 1, v.end(), back_inserter(*this));
602  return 2;
603  } else {
604  copy(v.begin(), v.end(), back_inserter(*this));
605  return 3;
606  }
607 }
608 
609 
610 void
612  if (size() > 0 && v.size() > 0 && back().distanceTo(v[0]) < sameThreshold) {
613  copy(v.begin() + 1, v.end(), back_inserter(*this));
614  } else {
615  copy(v.begin(), v.end(), back_inserter(*this));
616  }
617 }
618 
619 
621 PositionVector::getSubpart(SUMOReal beginOffset, SUMOReal endOffset) const {
622  PositionVector ret;
623  Position begPos = front();
624  if (beginOffset > POSITION_EPS) {
625  begPos = positionAtOffset(beginOffset);
626  }
627  Position endPos = back();
628  if (endOffset < length() - POSITION_EPS) {
629  endPos = positionAtOffset(endOffset);
630  }
631  ret.push_back(begPos);
632 
633  SUMOReal seen = 0;
634  const_iterator i = begin();
635  // skip previous segments
636  while ((i + 1) != end()
637  &&
638  seen + (*i).distanceTo(*(i + 1)) < beginOffset) {
639  seen += (*i).distanceTo(*(i + 1));
640  i++;
641  }
642  // append segments in between
643  while ((i + 1) != end()
644  &&
645  seen + (*i).distanceTo(*(i + 1)) < endOffset) {
646 
647  ret.push_back_noDoublePos(*(i + 1));
648  /*
649  if(ret.at(-1)!=*(i+1)) {
650  ret.push_back(*(i+1));
651  }
652  */
653  seen += (*i).distanceTo(*(i + 1));
654  i++;
655  }
656  // append end
657  ret.push_back_noDoublePos(endPos);
658  return ret;
659 }
660 
661 
663 PositionVector::getSubpart2D(SUMOReal beginOffset, SUMOReal endOffset) const {
664  PositionVector ret;
665  Position begPos = front();
666  if (beginOffset > POSITION_EPS) {
667  begPos = positionAtOffset2D(beginOffset);
668  }
669  Position endPos = back();
670  if (endOffset < length() - POSITION_EPS) {
671  endPos = positionAtOffset2D(endOffset);
672  }
673  ret.push_back(begPos);
674 
675  SUMOReal seen = 0;
676  const_iterator i = begin();
677  // skip previous segments
678  while ((i + 1) != end()
679  &&
680  seen + (*i).distanceTo2D(*(i + 1)) < beginOffset) {
681  seen += (*i).distanceTo2D(*(i + 1));
682  i++;
683  }
684  // append segments in between
685  while ((i + 1) != end()
686  &&
687  seen + (*i).distanceTo2D(*(i + 1)) < endOffset) {
688 
689  ret.push_back_noDoublePos(*(i + 1));
690  /*
691  if(ret.at(-1)!=*(i+1)) {
692  ret.push_back(*(i+1));
693  }
694  */
695  seen += (*i).distanceTo2D(*(i + 1));
696  i++;
697  }
698  // append end
699  ret.push_back_noDoublePos(endPos);
700  return ret;
701 }
702 
703 
704 void
706  // find minimum distance (from the begin)
707  size_t pos = 0;
708  SUMOReal dist = 1000000;
709  size_t currPos = 0;
711  GeomHelper::extrapolate_first(*(begin()), *(begin() + 1), 100),
712  *(begin() + 1));
713 // assert(currDist>=0);
714  if (currDist >= 0 && currDist < dist) {
715  dist = currDist;
716  pos = currPos;
717  }
718 
719  for (iterator i = begin(); i != end() - 1; i++, currPos++) {
720  SUMOReal currDist = GeomHelper::distancePointLine(p, *i, *(i + 1));
721  if (currDist >= 0 && currDist < dist) {
722  dist = currDist;
723  pos = currPos;
724  }
725  }
726  // remove leading items
727  for (size_t j = 0; j < pos; j++) {
728  erase(begin());
729  }
730  // replace first item by the new position
732  (*this)[0], (*this)[1], p);
733  if (lpos == GeomHelper::INVALID_OFFSET) {
734  return;
735  }
736  Position np = positionAtOffset(lpos);
737  if (np != *(begin())) {
738  erase(begin());
739  if (np != *(begin())) {
740  insert(begin(), p);
741  assert(size() > 1);
742  assert(*(begin()) != *(end() - 1));
743  }
744  }
745 }
746 
747 
749 PositionVector::getSubpartByIndex(int beginIndex, int count) const {
750  if (beginIndex < 0) {
751  beginIndex += (int)size();
752  }
753  assert(count > 0);
754  assert(beginIndex < (int)size());
755  assert(beginIndex + count <= (int)size());
756  PositionVector result;
757  for (int i = beginIndex; i < beginIndex + count; ++i) {
758  result.push_back((*this)[i]);
759  }
760  return result;
761 }
762 
763 
764 void
766  // find minimum distance (from the end)
767  size_t pos = 0;
768  SUMOReal dist = 1000000;
769  size_t currPos = 0;
771  *(end() - 1),
772  GeomHelper::extrapolate_second(*(end() - 2), *(end() - 1), 100));
773 // assert(currDist>=0);
774  if (currDist >= 0 && currDist < dist) {
775  dist = currDist;
776  pos = currPos;
777  }
778 
779  for (reverse_iterator i = rbegin(); i != rend() - 1; i++, currPos++) {
780  SUMOReal currDist = GeomHelper::distancePointLine(p, *(i), *(i + 1));
781  if (currDist >= 0 && currDist < dist) {
782  dist = currDist;
783  pos = currPos;
784  }
785  }
786  // remove trailing items
787  for (size_t j = 0; j < pos; j++) {
788  erase(end() - 1);
789  }
790  // replace last item by the new position
791  SUMOReal lpos =
793  (*this)[static_cast<int>(size()) - 1], (*this)[static_cast<int>(size()) - 2], p);
794  if (lpos == GeomHelper::INVALID_OFFSET) {
795  return;
796  }
798  length() - lpos);
799  if (np != *(end() - 1)) {
800  erase(end() - 1);
801  if (np != *(end() - 1)) {
802  push_back(np);
803  assert(size() > 1);
804  assert(*(begin()) != *(end() - 1));
805  }
806  }
807 }
808 
809 
810 SUMOReal
812  Line tmp(front(), back());
813  return tmp.atan2Angle();
814 }
815 
816 
817 void
819  if (i >= 0) {
820  erase(begin() + i);
821  } else {
822  erase(end() + i);
823  }
824 }
825 
826 
827 SUMOReal
828 PositionVector::nearest_offset_to_point2D(const Position& p, bool perpendicular) const {
830  SUMOReal nearestPos = -1;
831  SUMOReal seen = 0;
832  for (const_iterator i = begin(); i != end() - 1; i++) {
833  const SUMOReal pos =
834  GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, perpendicular);
835  const SUMOReal dist = pos == GeomHelper::INVALID_OFFSET ? minDist : p.distanceTo2D(Line(*i, *(i + 1)).getPositionAtDistance2D(pos));
836  if (dist < minDist) {
837  nearestPos = pos + seen;
838  minDist = dist;
839  }
840  if (perpendicular && i != begin() && pos == GeomHelper::INVALID_OFFSET) {
841  // even if perpendicular is set we still need to check the distance to the inner points
842  const SUMOReal cornerDist = p.distanceTo2D(*i);
843  if (cornerDist < minDist) {
844  const SUMOReal pos1 =
845  GeomHelper::nearest_offset_on_line_to_point2D(*(i - 1), *i, p, false);
846  const SUMOReal pos2 =
847  GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, false);
848  if (pos1 == (*(i - 1)).distanceTo2D(*i) && pos2 == 0.) {
849  nearestPos = seen;
850  minDist = cornerDist;
851  }
852  }
853  }
854  seen += (*i).distanceTo2D(*(i + 1));
855  }
856  return nearestPos;
857 }
858 
859 
860 Position
862  // XXX this duplicates most of the code in nearest_offset_to_point2D. It should be refactored
863  if (extend) {
864  PositionVector extended = *this;
865  const SUMOReal dist = 2 * distance(p);
866  extended.extrapolate(dist);
867  return extended.transformToVectorCoordinates(p) - Position(dist, 0);
868  }
870  SUMOReal nearestPos = -1;
871  SUMOReal seen = 0;
872  int sign = 1;
873  for (const_iterator i = begin(); i != end() - 1; i++) {
874  const SUMOReal pos =
875  GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, true);
876  const SUMOReal dist = pos < 0 ? minDist : p.distanceTo2D(Line(*i, *(i + 1)).getPositionAtDistance(pos));
877  if (dist < minDist) {
878  nearestPos = pos + seen;
879  minDist = dist;
880  sign = isLeft(*i, *(i + 1), p) >= 0 ? -1 : 1;
881  }
882  if (i != begin() && pos == GeomHelper::INVALID_OFFSET) {
883  // even if perpendicular is set we still need to check the distance to the inner points
884  const SUMOReal cornerDist = p.distanceTo2D(*i);
885  if (cornerDist < minDist) {
886  const SUMOReal pos1 =
887  GeomHelper::nearest_offset_on_line_to_point2D(*(i - 1), *i, p, false);
888  const SUMOReal pos2 =
889  GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, false);
890  if (pos1 == (*(i - 1)).distanceTo2D(*i) && pos2 == 0.) {
891  nearestPos = seen;
892  minDist = cornerDist;
893  sign = isLeft(*(i - 1), *i, p) >= 0 ? -1 : 1;
894  }
895  }
896  }
897  seen += (*i).distanceTo2D(*(i + 1));
898  }
899  if (nearestPos != -1) {
900  return Position(nearestPos, sign * minDist);
901  } else {
902  return Position::INVALID;
903  }
904 }
905 
906 
907 int
909  assert(size() > 0);
911  SUMOReal dist;
912  int closest = 0;
913  for (int i = 0; i < (int)size(); i++) {
914  dist = p.distanceTo((*this)[i]);
915  if (dist < minDist) {
916  closest = i;
917  minDist = dist;
918  }
919  }
920  return closest;
921 }
922 
923 
924 int
926  Position outIntersection = Position();
928  SUMOReal dist;
929  int insertionIndex = 1;
930  for (int i = 0; i < (int)size() - 1; i++) {
931  dist = GeomHelper::closestDistancePointLine2D(p, (*this)[i], (*this)[i + 1], outIntersection);
932  if (dist < minDist) {
933  insertionIndex = i + 1;
934  minDist = dist;
935  }
936  }
937  insertAt(insertionIndex, p);
938  return insertionIndex;
939 }
940 
941 
942 std::vector<SUMOReal>
944  std::vector<SUMOReal> ret;
945  for (const_iterator i = other.begin(); i != other.end() - 1; i++) {
946  std::vector<SUMOReal> atSegment = intersectsAtLengths2D(Line(*i, *(i + 1)));
947  copy(atSegment.begin(), atSegment.end(), back_inserter(ret));
948  }
949  return ret;
950 }
951 
952 
953 std::vector<SUMOReal>
955  std::vector<SUMOReal> ret;
956  SUMOReal pos = 0;
957  for (const_iterator i = begin(); i != end() - 1; i++) {
958  Line l((*i), *(i + 1));
959  if (GeomHelper::intersects(l.p1(), l.p2(), line.p1(), line.p2())) {
960  Position p =
961  GeomHelper::intersection_position2D(l.p1(), l.p2(), line.p1(), line.p2());
962  SUMOReal atLength = p.distanceTo2D(l.p1());
963  ret.push_back(atLength + pos);
964  }
965  pos += l.length2D();
966  }
967  return ret;
968 }
969 
970 
971 void
973  assert(size() > 1);
974  Position nb =
975  GeomHelper::extrapolate_first((*this)[0], (*this)[1], val);
976  Position ne =
978  (*this)[static_cast<int>(size()) - 2], (*this)[static_cast<int>(size()) - 1], val);
979  erase(begin());
980  push_front(nb);
981  erase(end() - 1);
982  push_back(ne);
983 }
984 
985 
988  PositionVector ret;
989  for (const_reverse_iterator i = rbegin(); i != rend(); i++) {
990  ret.push_back(*i);
991  }
992  return ret;
993 }
994 
995 
996 void
998  if (size() < 2) {
999  return;
1000  }
1001  PositionVector shape;
1002  for (int i = 0; i < static_cast<int>(size()); i++) {
1003  if (i == 0) {
1004  Position from = (*this)[i];
1005  Position to = (*this)[i + 1];
1006  std::pair<SUMOReal, SUMOReal> offsets =
1007  GeomHelper::getNormal90D_CW(from, to, amount);
1008  shape.push_back(Position(from.x() - offsets.first,
1009  from.y() - offsets.second, from.z()));
1010  } else if (i == static_cast<int>(size()) - 1) {
1011  Position from = (*this)[i - 1];
1012  Position to = (*this)[i];
1013  std::pair<SUMOReal, SUMOReal> offsets =
1014  GeomHelper::getNormal90D_CW(from, to, amount);
1015  shape.push_back(Position(to.x() - offsets.first,
1016  to.y() - offsets.second, to.z()));
1017  } else {
1018  Position from = (*this)[i - 1];
1019  Position me = (*this)[i];
1020  Position to = (*this)[i + 1];
1021  Line fromMe(from, me);
1022  fromMe.extrapolateBy2D(me.distanceTo2D(to));
1023  const double extrapolateDev = fromMe.p2().distanceTo2D(to);
1024  if (fabs(extrapolateDev) < POSITION_EPS) {
1025  // parallel case, just shift the middle point
1026  std::pair<SUMOReal, SUMOReal> off =
1027  GeomHelper::getNormal90D_CW(from, to, amount);
1028  shape.push_back(Position(me.x() - off.first, me.y() - off.second, me.z()));
1029  continue;
1030  }
1031  if (fabs(extrapolateDev - 2 * me.distanceTo2D(to)) < POSITION_EPS) {
1032  // counterparallel case, just shift the middle point
1033  Line fromMe(from, me);
1034  fromMe.extrapolateBy2D(amount);
1035  shape.push_back(fromMe.p2());
1036  continue;
1037  }
1038  std::pair<SUMOReal, SUMOReal> offsets =
1039  GeomHelper::getNormal90D_CW(from, me, amount);
1040  std::pair<SUMOReal, SUMOReal> offsets2 =
1041  GeomHelper::getNormal90D_CW(me, to, amount);
1042  Line l1(
1043  Position(from.x() - offsets.first, from.y() - offsets.second),
1044  Position(me.x() - offsets.first, me.y() - offsets.second));
1045  l1.extrapolateBy2D(100);
1046  Line l2(
1047  Position(me.x() - offsets2.first, me.y() - offsets2.second),
1048  Position(to.x() - offsets2.first, to.y() - offsets2.second));
1049  l2.extrapolateBy2D(100);
1050  if (l1.intersects(l2)) {
1051  shape.push_back(l1.intersectsAt(l2));
1052  } else {
1053  throw InvalidArgument("no line intersection");
1054  }
1055  }
1056  }
1057 
1058  /*
1059  ContType newCont;
1060  std::pair<SUMOReal, SUMOReal> p;
1061  Position newPos;
1062  // first point
1063  newPos = (*(begin()));
1064  p = GeomHelper::getNormal90D_CW(*(begin()), *(begin()+1), amount);
1065  newPos.add(p.first, p.second);
1066  newCont.push_back(newPos);
1067  // middle points
1068  for(const_iterator i=begin()+1; i!=end()-1; i++) {
1069  std::pair<SUMOReal, SUMOReal> oldp = p;
1070  newPos = *i;
1071  newPos.add(p.first, p.second);
1072  newCont.push_back(newPos);
1073  p = GeomHelper::getNormal90D_CW(*i, *(i+1), amount);
1074  // Position newPos(*i);
1075  // newPos.add((p.first+oldp.first)/2.0, (p.second+oldp.second)/2.0);
1076  // newCont.push_back(newPos);
1077  }
1078  // last point
1079  newPos = (*(end()-1));
1080  newPos.add(p.first, p.second);
1081  newCont.push_back(newPos);
1082  myCont = newCont;
1083  */
1084  *this = shape;
1085 }
1086 
1087 
1088 Line
1089 PositionVector::lineAt(int pos) const {
1090  assert((int)size() > pos + 1);
1091  return Line((*this)[pos], (*this)[pos + 1]);
1092 }
1093 
1094 
1095 Line
1097  return lineAt(0);
1098 }
1099 
1100 
1101 Line
1103  return lineAt((int)size() - 2);
1104 }
1105 
1106 
1107 void
1109  if ((*this)[0] == back()) {
1110  return;
1111  }
1112  push_back((*this)[0]);
1113 }
1114 
1115 
1116 std::vector<SUMOReal>
1117 PositionVector::distances(const PositionVector& s, bool perpendicular) const {
1118  std::vector<SUMOReal> ret;
1119  const_iterator i;
1120  for (i = begin(); i != end(); i++) {
1121  const SUMOReal dist = s.distance(*i, perpendicular);
1122  if (dist != GeomHelper::INVALID_OFFSET) {
1123  ret.push_back(dist);
1124  }
1125  }
1126  for (i = s.begin(); i != s.end(); i++) {
1127  const SUMOReal dist = distance(*i, perpendicular);
1128  if (dist != GeomHelper::INVALID_OFFSET) {
1129  ret.push_back(dist);
1130  }
1131  }
1132  return ret;
1133 }
1134 
1135 
1136 SUMOReal
1137 PositionVector::distance(const Position& p, bool perpendicular) const {
1138  if (size() == 0) {
1140  } else if (size() == 1) {
1141  return front().distanceTo(p);
1142  }
1143  const SUMOReal nearestOffset = nearest_offset_to_point2D(p, perpendicular);
1144  if (nearestOffset == GeomHelper::INVALID_OFFSET) {
1146  } else {
1147  return p.distanceTo2D(positionAtOffset2D(nearestOffset));
1148  }
1149 }
1150 
1151 
1152 void
1153 PositionVector::insertAt(int index, const Position& p) {
1154  if (index >= 0) {
1155  insert(begin() + index, p);
1156  } else {
1157  insert(end() + index, p);
1158  }
1159 }
1160 
1161 
1162 void
1163 PositionVector::replaceAt(int index, const Position& p) {
1164  assert(index < static_cast<int>(size()));
1165  assert(index + static_cast<int>(size()) >= 0);
1166  if (index >= 0) {
1167  (*this)[index] = p;
1168  } else {
1169  (*this)[index + static_cast<int>(size())] = p;
1170  }
1171 }
1172 
1173 
1174 void
1176  if (size() == 0 || !p.almostSame(back())) {
1177  push_back(p);
1178  }
1179 }
1180 
1181 
1182 void
1184  if (size() == 0 || !p.almostSame(front())) {
1185  insert(begin(), p);
1186  }
1187 }
1188 
1189 
1190 bool
1192  return size() >= 2 && (*this)[0] == back();
1193 }
1194 
1195 
1196 void
1197 PositionVector::removeDoublePoints(SUMOReal minDist, bool assertLength) {
1198  if (size() > 1) {
1199  iterator last = begin();
1200  for (iterator i = begin() + 1; i != end() && (!assertLength || size() > 2);) {
1201  if (last->almostSame(*i, minDist)) {
1202  i = erase(i);
1203  } else {
1204  last = i;
1205  ++i;
1206  }
1207  }
1208  }
1209 }
1210 
1211 
1212 void
1214  if (size() > 2) {
1215  Position& last = front();
1216  for (iterator i = begin() + 1; i != end() - 1;) {
1217  if (GeomHelper::distancePointLine(*i, last, *(i + 1)) < 0.001) {
1218  i = erase(i);
1219  } else {
1220  last = *i;
1221  ++i;
1222  }
1223  }
1224  }
1225 }
1226 
1227 
1228 bool
1230  if (size() == v2.size()) {
1231  for (int i = 0; i < (int)size(); i++) {
1232  if ((*this)[i] != v2[i]) {
1233  return false;
1234  }
1235  }
1236  return true;
1237  } else {
1238  return false;
1239  }
1240 }
1241 
1242 
1243 
1244 /****************************************************************************/
1245 
SUMOReal length2D() const
Definition: Line.cpp:186
SUMOReal atan2DegreeAngle() const
Definition: Line.cpp:152
static std::pair< SUMOReal, SUMOReal > getNormal90D_CW(const Position &beg, const Position &end, SUMOReal length, SUMOReal wanted_offset)
Definition: GeomHelper.cpp:373
const Position & p2() const
Definition: Line.cpp:95
static SUMOReal Angle2D(SUMOReal x1, SUMOReal y1, SUMOReal x2, SUMOReal y2)
Definition: GeomHelper.cpp:224
SUMOReal distance(const Position &p, bool perpendicular=false) const
void pruneFromBeginAt(const Position &p)
static Position intersection_position2D(const Position &p11, const Position &p12, const Position &p21, const Position &p22)
returns the intersection point of the (infinite) lines p11,p12 and p21,p22. If the given lines are pa...
Definition: GeomHelper.cpp:203
SUMOReal nearest_offset_to_point2D(const Position &p, bool perpendicular=true) const
PositionVector getSubpart2D(SUMOReal beginOffset, SUMOReal endOffset) const
void sortAsPolyCWByAngle()
void replaceAt(int index, const Position &by)
SUMOReal intersectsAtLength2D(const Line &v)
returns distance between myP1 and intersection or -1 if line segments do not intersect ...
Definition: Line.cpp:229
void insertAt(int index, const Position &p)
#define M_PI
Definition: angles.h:37
SUMOReal atan2DegreeSlope() const
Definition: Line.cpp:168
Position getPositionAtDistance2D(SUMOReal offset) const
Definition: Line.cpp:114
Line getEndLine() const
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.
static Position extrapolate_second(const Position &p1, const Position &p2, SUMOReal length)
Definition: GeomHelper.cpp:253
SUMOReal distanceTo(const Position &p2) const
returns the euclidean distance in 3 dimension
Definition: Position.h:229
void eraseAt(int i)
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:223
static Position extrapolate_first(const Position &p1, const Position &p2, SUMOReal length)
Definition: GeomHelper.cpp:245
SUMOReal beginEndAngle() const
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.
SUMOReal length2D() const
Returns the length.
std::vector< SUMOReal > distances(const PositionVector &s, bool perpendicular=false) const
Position getPositionAtDistance(SUMOReal offset) const
Definition: Line.cpp:101
static SUMOReal nearest_offset_on_line_to_point2D(const Position &l1, const Position &l2, const Position &p, bool perpendicular=true)
Definition: GeomHelper.cpp:261
Line lineAt(int pos) const
static bool intersects(const Position &p11, const Position &p12, const Position &p21, const Position &p22)
return whether given lines intersect
Definition: GeomHelper.cpp:145
void push_front_noDoublePos(const Position &p)
const Position & p1() const
Definition: Line.cpp:89
#define max(a, b)
Definition: polyfonts.c:65
void reshiftRotate(SUMOReal xoff, SUMOReal yoff, SUMOReal rot)
A point in 2D or 3D with translation and scaling methods.
Definition: Position.h:46
Position pop_front()
Removes and returns the position at the fron of the list.
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
void push_front(const Position &p)
Puts the given position at the front of the list.
static SUMOReal distancePointLine(const Position &point, const Position &lineStart, const Position &lineEnd)
Definition: GeomHelper.cpp:288
void move2side(SUMOReal amount)
Definition: Line.cpp:127
SUMOReal z() const
Returns the z-position.
Definition: Position.h:73
static SUMOReal closestDistancePointLine2D(const Position &point, const Position &lineStart, const Position &lineEnd, Position &outIntersection)
Definition: GeomHelper.cpp:312
Position positionAtOffset(SUMOReal pos, SUMOReal lateralOffset=0) const
Returns the position at the given length.
Definition: Line.h:51
#define POSITION_EPS
Definition: config.h:189
int insertAtClosest(const Position &p)
std::string toString(const T &t, std::streamsize accuracy=OUTPUT_ACCURACY)
Definition: ToString.h:53
Position intersectsAtPoint(const Position &p1, const Position &p2) const
SUMOReal atan2Angle() const
Definition: Line.cpp:146
bool intersects(const Line &l) const
Definition: Line.cpp:180
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
void extrapolate(SUMOReal val)
PositionVector()
Constructor.
void extrapolateBy(SUMOReal length)
Definition: Line.cpp:60
SUMOReal length() const
Returns the length.
SUMOReal rotationDegreeAtOffset(SUMOReal pos) const
Returns the rotation at the given length.
void push_back(const PositionVector &p)
Appends all positions from the given vector.
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.
PositionVector intersectionPoints2D(const Line &line) const
int appendWithCrossingPoint(const PositionVector &v)
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...
Line getBegLine() const
void pruneFromEndAt(const Position &p)
Position getLineCenter() const
void extrapolateBy2D(SUMOReal length)
Definition: Line.cpp:69
SUMOReal distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
Definition: Position.h:240
void move2side(SUMOReal amount)
#define SUMOReal
Definition: config.h:218
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
Position intersectsAt(const Line &l) const
Definition: Line.cpp:174
PositionVector getSubpart(SUMOReal beginOffset, SUMOReal endOffset) const
void append(const PositionVector &v, SUMOReal sameThreshold=2.0)
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
std::ostream & operator<<(std::ostream &os, const PositionVector &geom)
static const Position INVALID
Definition: Position.h:262
Position transformToVectorCoordinates(const Position &p, bool extend=false) const
return position p within the length-wise coordinate system defined by this position vector...