SUMO - Simulation of Urban MObility
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups 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-sim.org/
13 // Copyright (C) 2001-2014 DLR (http://www.dlr.de/) and contributors
14 /****************************************************************************/
15 //
16 // This file is part of SUMO.
17 // SUMO is free software: you can redistribute it and/or modify
18 // it under the terms of the GNU General Public License as published by
19 // the Free Software Foundation, either version 3 of the License, or
20 // (at your option) any later version.
21 //
22 /****************************************************************************/
23 
24 
25 // ===========================================================================
26 // included modules
27 // ===========================================================================
28 #ifdef _MSC_VER
29 #include <windows_config.h>
30 #else
31 #include <config.h>
32 #endif
33 
34 #include <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 // ===========================================================================
58 // method definitions
59 // ===========================================================================
61 
62 
63 PositionVector::PositionVector(const std::vector<Position>& v) {
64  std::copy(v.begin(), v.end(), std::back_inserter(*this));
65 }
66 
67 
69 
70 
71 // ------------ Adding items to the container
72 void
74  copy(p.begin(), p.end(), back_inserter(*this));
75 }
76 
77 
78 void
80  insert(begin(), p);
81 }
82 
83 
86  Position first = front();
87  erase(begin());
88  return first;
89 }
90 
91 
92 bool
93 PositionVector::around(const Position& p, SUMOReal offset) const {
94  if (offset != 0) {
95  PositionVector tmp(*this);
96  tmp.scaleAbsolute(offset);
97  return tmp.around(p);
98  }
99  SUMOReal angle = 0;
100  for (const_iterator i = begin(); i != end() - 1; i++) {
101  Position p1(
102  (*i).x() - p.x(),
103  (*i).y() - p.y());
104  Position p2(
105  (*(i + 1)).x() - p.x(),
106  (*(i + 1)).y() - p.y());
107  angle += GeomHelper::Angle2D(p1.x(), p1.y(), p2.x(), p2.y());
108  }
109  Position p1(
110  (*(end() - 1)).x() - p.x(),
111  (*(end() - 1)).y() - p.y());
112  Position p2(
113  (*(begin())).x() - p.x(),
114  (*(begin())).y() - p.y());
115  angle += GeomHelper::Angle2D(p1.x(), p1.y(), p2.x(), p2.y());
116  return (!(fabs(angle) < M_PI));
117 }
118 
119 
120 bool
122  for (const_iterator i = begin(); i != end() - 1; i++) {
123  if (poly.around(*i, offset)) {
124  return true;
125  }
126  }
127  return false;
128 }
129 
130 
131 bool
132 PositionVector::intersects(const Position& p1, const Position& p2) const {
133  if (size() < 2) {
134  return false;
135  }
136  for (const_iterator i = begin(); i != end() - 1; i++) {
137  if (GeomHelper::intersects(*i, *(i + 1), p1, p2)) {
138  return true;
139  }
140  }
141  //return GeomHelper::intersects(*(end()-1), *(begin()), p1, p2);
142  return false;
143 }
144 
145 
146 bool
148  if (size() < 2) {
149  return false;
150  }
151  for (const_iterator i = begin(); i != end() - 1; i++) {
152  if (v1.intersects(*i, *(i + 1))) {
153  return true;
154  }
155  }
156  //return v1.intersects(*(end()-1), *(begin()));
157  return false;
158 }
159 
160 
161 Position
163  const Position& p2) const {
164  for (const_iterator i = begin(); i != end() - 1; i++) {
165  if (GeomHelper::intersects(*i, *(i + 1), p1, p2)) {
166  return GeomHelper::intersection_position2D(*i, *(i + 1), p1, p2);
167  }
168  }
169  return Position(-1, -1);
170 }
171 
172 
173 Position
175  for (const_iterator i = begin(); i != end() - 1; i++) {
176  if (v1.intersects(*i, *(i + 1))) {
177  return v1.intersectsAtPoint(*i, *(i + 1));
178  }
179  }
180  /*
181  if(v1.intersects(*(end()-1), *(begin()))) {
182  return v1.intersectsAtPoint(*(end()-1), *(begin()));
183  }
184  */
185  return Position(-1, -1);
186 }
187 
188 
189 const Position&
190 PositionVector::operator[](int index) const {
191  if (index >= 0) {
192  return at(index);
193  } else {
194  return at(size() + index);
195  }
196 }
197 
198 
199 Position&
201  if (index >= 0) {
202  return at(index);
203  } else {
204  return at(size() + index);
205  }
206 }
207 
208 
209 Position
211  const_iterator i = begin();
212  SUMOReal seenLength = 0;
213  do {
214  const SUMOReal nextLength = (*i).distanceTo(*(i + 1));
215  if (seenLength + nextLength > pos) {
216  return positionAtOffset(*i, *(i + 1), pos - seenLength);
217  }
218  seenLength += nextLength;
219  } while (++i != end() - 1);
220  return back();
221 }
222 
223 
224 Position
226  const_iterator i = begin();
227  SUMOReal seenLength = 0;
228  do {
229  const SUMOReal nextLength = (*i).distanceTo2D(*(i + 1));
230  if (seenLength + nextLength > pos) {
231  return positionAtOffset2D(*i, *(i + 1), pos - seenLength);
232  }
233  seenLength += nextLength;
234  } while (++i != end() - 1);
235  return back();
236 }
237 
238 
239 SUMOReal
241  const_iterator i = begin();
242  SUMOReal seenLength = 0;
243  do {
244  SUMOReal nextLength = (*i).distanceTo(*(i + 1));
245  if (seenLength + nextLength > pos) {
246  Line l(*i, *(i + 1));
247  return l.atan2DegreeAngle();
248  }
249  seenLength += nextLength;
250  } while (++i != end() - 1);
251  Line l(*(end() - 2), *(end() - 1));
252  return l.atan2DegreeAngle();
253 }
254 
255 SUMOReal
257  const_iterator i = begin();
258  SUMOReal seenLength = 0;
259  do {
260  SUMOReal nextLength = (*i).distanceTo(*(i + 1));
261  if (seenLength + nextLength > pos) {
262  Line l(*i, *(i + 1));
263  return l.atan2DegreeSlope();
264  }
265  seenLength += nextLength;
266  } while (++i != end() - 1);
267  Line l(*(end() - 2), *(end() - 1));
268  return l.atan2DegreeSlope();
269 }
270 
271 Position
273  const Position& p2,
274  SUMOReal pos) {
275  const SUMOReal dist = p1.distanceTo(p2);
276  if (dist < pos) {
277  return Position(-1, -1);
278  }
279  return p1 + (p2 - p1) * (pos / dist);
280 }
281 
282 
283 Position
285  const Position& p2,
286  SUMOReal pos) {
287  const SUMOReal dist = p1.distanceTo2D(p2);
288  if (dist < pos) {
289  return Position(-1, -1);
290  }
291  return p1 + (p2 - p1) * (pos / dist);
292 }
293 
294 
295 Boundary
297  Boundary ret;
298  for (const_iterator i = begin(); i != end(); i++) {
299  ret.add(*i);
300  }
301  return ret;
302 }
303 
304 
305 Position
307  SUMOReal x = 0;
308  SUMOReal y = 0;
309  for (const_iterator i = begin(); i != end(); i++) {
310  x += (*i).x();
311  y += (*i).y();
312  }
313  return Position(x / (SUMOReal) size(), y / (SUMOReal) size());
314 }
315 
316 
317 Position
319  PositionVector tmp = *this;
320  if (!isClosed()) { // make sure its closed
321  tmp.push_back(tmp[0]);
322  }
323  const int endIndex = (int)tmp.size() - 1;
324  SUMOReal div = 0; // 6 * area including sign
325  SUMOReal x = 0;
326  SUMOReal y = 0;
327  if (tmp.area() != 0) { // numerical instability ?
328  // http://en.wikipedia.org/wiki/Polygon
329  for (int i = 0; i < endIndex; i++) {
330  const SUMOReal z = tmp[i].x() * tmp[i + 1].y() - tmp[i + 1].x() * tmp[i].y();
331  div += z; // area formula
332  x += (tmp[i].x() + tmp[i + 1].x()) * z;
333  y += (tmp[i].y() + tmp[i + 1].y()) * z;
334  }
335  div *= 3; // 6 / 2, the 2 comes from the area formula
336  return Position(x / div, y / div);
337  } else {
338  // compute by decomposing into line segments
339  // http://en.wikipedia.org/wiki/Centroid#By_geometric_decomposition
340  SUMOReal lengthSum = 0;
341  for (int i = 0; i < endIndex; i++) {
342  SUMOReal length = tmp[i].distanceTo(tmp[i + 1]);
343  x += (tmp[i].x() + tmp[i + 1].x()) * length / 2;
344  y += (tmp[i].y() + tmp[i + 1].y()) * length / 2;
345  lengthSum += length;
346  }
347  return Position(x / lengthSum, y / lengthSum);
348  }
349 }
350 
351 
352 void
354  Position centroid = getCentroid();
355  for (int i = 0; i < static_cast<int>(size()); i++) {
356  (*this)[i] = centroid + (((*this)[i] - centroid) * factor);
357  }
358 }
359 
360 
361 void
363  Position centroid = getCentroid();
364  for (int i = 0; i < static_cast<int>(size()); i++) {
365  (*this)[i] = centroid + (((*this)[i] - centroid) + offset);
366  }
367 }
368 
369 
370 Position
372  if (size() == 1) {
373  return (*this)[0];
374  }
375  return positionAtOffset(SUMOReal((length() / 2.)));
376 }
377 
378 
379 SUMOReal
381  SUMOReal len = 0;
382  for (const_iterator i = begin(); i != end() - 1; i++) {
383  len += (*i).distanceTo(*(i + 1));
384  }
385  return len;
386 }
387 
388 SUMOReal
390  SUMOReal len = 0;
391  for (const_iterator i = begin(); i != end() - 1; i++) {
392  len += (*i).distanceTo2D(*(i + 1));
393  }
394  return len;
395 }
396 
397 
398 SUMOReal
400  SUMOReal area = 0;
401  PositionVector tmp = *this;
402  if (!isClosed()) { // make sure its closed
403  tmp.push_back(tmp[0]);
404  }
405  const int endIndex = (int)tmp.size() - 1;
406  // http://en.wikipedia.org/wiki/Polygon
407  for (int i = 0; i < endIndex; i++) {
408  area += tmp[i].x() * tmp[i + 1].y() - tmp[i + 1].x() * tmp[i].y();
409  }
410  if (area < 0) { // we whether we had cw or ccw order
411  area *= -1;
412  }
413  return area / 2;
414 }
415 
416 
417 bool
419  for (const_iterator i = begin(); i != end() - 1; i++) {
420  if (poly.around(*i, offset)) {
421  return true;
422  }
423  }
424  return false;
425 }
426 
427 
428 bool
429 PositionVector::crosses(const Position& p1, const Position& p2) const {
430  return intersects(p1, p2);
431 }
432 
433 
434 std::pair<PositionVector, PositionVector>
436  if (size() < 2) {
437  throw InvalidArgument("Vector to short for splitting");
438  }
439  if (where <= POSITION_EPS || where >= length() - POSITION_EPS) {
440  WRITE_WARNING("Splitting vector close to end (pos: " + toString(where) + ", length: " + toString(length()) + ")");
441  }
442  PositionVector first, second;
443  first.push_back((*this)[0]);
444  SUMOReal seen = 0;
445  const_iterator it = begin() + 1;
446  SUMOReal next = first.back().distanceTo(*it);
447  // see how many points we can add to first
448  while (where >= seen + next + POSITION_EPS) {
449  seen += next;
450  first.push_back(*it);
451  it++;
452  next = first.back().distanceTo(*it);
453  }
454  if (fabs(where - (seen + next)) > POSITION_EPS || it == end() - 1) {
455  // we need to insert a new point because 'where' is not close to an
456  // existing point or it is to close to the endpoint
457  Line tmpL(first.back(), *it);
458  Position p = tmpL.getPositionAtDistance(where - seen);
459  first.push_back(p);
460  second.push_back(p);
461  } else {
462  first.push_back(*it);
463  }
464  // add the remaining points to second
465  for (; it != end(); it++) {
466  second.push_back(*it);
467  }
468  assert(first.size() >= 2);
469  assert(second.size() >= 2);
470  assert(first.back() == second.front());
471  assert(fabs(first.length() + second.length() - length()) < 2 * POSITION_EPS);
472  return std::pair<PositionVector, PositionVector>(first, second);
473 }
474 
475 
476 std::ostream&
477 operator<<(std::ostream& os, const PositionVector& geom) {
478  for (PositionVector::const_iterator i = geom.begin(); i != geom.end(); i++) {
479  if (i != geom.begin()) {
480  os << " ";
481  }
482  os << (*i);
483  }
484  return os;
485 }
486 
487 
488 void
490  std::sort(begin(), end(), as_poly_cw_sorter());
491 }
492 
493 
494 void
496  for (int i = 0; i < static_cast<int>(size()); i++) {
497  (*this)[i].add(xoff, yoff, zoff);
498  }
499 }
500 
501 
502 void
504  for (int i = 0; i < static_cast<int>(size()); i++) {
505  (*this)[i].reshiftRotate(xoff, yoff, rot);
506  }
507 }
508 
509 
510 int
512  const Position& p2) const {
513  return atan2(p1.x(), p1.y()) < atan2(p2.x(), p2.y());
514 }
515 
516 
517 
518 void
520  std::sort(begin(), end(), increasing_x_y_sorter());
521 }
522 
523 
524 
526 
527 
528 
529 int
531  const Position& p2) const {
532  if (p1.x() != p2.x()) {
533  return p1.x() < p2.x();
534  }
535  return p1.y() < p2.y();
536 }
537 
538 
539 
540 SUMOReal
542  const Position& P2) const {
543  return (P1.x() - P0.x()) * (P2.y() - P0.y()) - (P2.x() - P0.x()) * (P1.y() - P0.y());
544 }
545 
546 
549  PositionVector ret = *this;
550  ret.sortAsPolyCWByAngle();
551  return simpleHull_2D(ret);
552 }
553 
554 
557  PositionVector ret;
558  for (const_iterator i = begin(); i != end() - 1; i++) {
559  if (GeomHelper::intersects(*i, *(i + 1), line.p1(), line.p2())) {
561  *i, *(i + 1), line.p1(), line.p2()));
562  }
563  }
564  return ret;
565 }
566 
567 
568 int
570  if (back().distanceTo(v[0]) < 2) { // !!! heuristic
571  copy(v.begin() + 1, v.end(), back_inserter(*this));
572  return 1;
573  }
574  //
575  Line l1((*this)[static_cast<int>(size()) - 2], back());
576  l1.extrapolateBy(100);
577  Line l2(v[0], v[1]);
578  l2.extrapolateBy(100);
579  if (l1.intersects(l2) && l1.intersectsAtLength2D(l2) < l1.length2D() - 100) { // !!! heuristic
580  Position p = l1.intersectsAt(l2);
581  (*this)[static_cast<int>(size()) - 1] = p;
582  copy(v.begin() + 1, v.end(), back_inserter(*this));
583  return 2;
584  } else {
585  copy(v.begin(), v.end(), back_inserter(*this));
586  return 3;
587  }
588 }
589 
590 
591 void
593  if (back().distanceTo(v[0]) < 2) {
594  copy(v.begin() + 1, v.end(), back_inserter(*this));
595  } else {
596  copy(v.begin(), v.end(), back_inserter(*this));
597  }
598 }
599 
600 
602 PositionVector::getSubpart(SUMOReal beginOffset, SUMOReal endOffset) const {
603  PositionVector ret;
604  Position begPos = front();
605  if (beginOffset > POSITION_EPS) {
606  begPos = positionAtOffset(beginOffset);
607  }
608  Position endPos = back();
609  if (endOffset < length() - POSITION_EPS) {
610  endPos = positionAtOffset(endOffset);
611  }
612  ret.push_back(begPos);
613 
614  SUMOReal seen = 0;
615  const_iterator i = begin();
616  // skip previous segments
617  while ((i + 1) != end()
618  &&
619  seen + (*i).distanceTo(*(i + 1)) < beginOffset) {
620  seen += (*i).distanceTo(*(i + 1));
621  i++;
622  }
623  // append segments in between
624  while ((i + 1) != end()
625  &&
626  seen + (*i).distanceTo(*(i + 1)) < endOffset) {
627 
628  ret.push_back_noDoublePos(*(i + 1));
629  /*
630  if(ret.at(-1)!=*(i+1)) {
631  ret.push_back(*(i+1));
632  }
633  */
634  seen += (*i).distanceTo(*(i + 1));
635  i++;
636  }
637  // append end
638  ret.push_back_noDoublePos(endPos);
639  return ret;
640 }
641 
642 
644 PositionVector::getSubpart2D(SUMOReal beginOffset, SUMOReal endOffset) const {
645  PositionVector ret;
646  Position begPos = front();
647  if (beginOffset > POSITION_EPS) {
648  begPos = positionAtOffset2D(beginOffset);
649  }
650  Position endPos = back();
651  if (endOffset < length() - POSITION_EPS) {
652  endPos = positionAtOffset2D(endOffset);
653  }
654  ret.push_back(begPos);
655 
656  SUMOReal seen = 0;
657  const_iterator i = begin();
658  // skip previous segments
659  while ((i + 1) != end()
660  &&
661  seen + (*i).distanceTo2D(*(i + 1)) < beginOffset) {
662  seen += (*i).distanceTo2D(*(i + 1));
663  i++;
664  }
665  // append segments in between
666  while ((i + 1) != end()
667  &&
668  seen + (*i).distanceTo2D(*(i + 1)) < endOffset) {
669 
670  ret.push_back_noDoublePos(*(i + 1));
671  /*
672  if(ret.at(-1)!=*(i+1)) {
673  ret.push_back(*(i+1));
674  }
675  */
676  seen += (*i).distanceTo2D(*(i + 1));
677  i++;
678  }
679  // append end
680  ret.push_back_noDoublePos(endPos);
681  return ret;
682 }
683 
684 
685 void
687  // find minimum distance (from the begin)
688  size_t pos = 0;
689  SUMOReal dist = 1000000;
690  size_t currPos = 0;
692  GeomHelper::extrapolate_first(*(begin()), *(begin() + 1), 100),
693  *(begin() + 1));
694 // assert(currDist>=0);
695  if (currDist >= 0 && currDist < dist) {
696  dist = currDist;
697  pos = currPos;
698  }
699 
700  for (iterator i = begin(); i != end() - 1; i++, currPos++) {
701  SUMOReal currDist = GeomHelper::distancePointLine(p, *i, *(i + 1));
702  if (currDist >= 0 && currDist < dist) {
703  dist = currDist;
704  pos = currPos;
705  }
706  }
707  // remove leading items
708  for (size_t j = 0; j < pos; j++) {
709  erase(begin());
710  }
711  // replace first item by the new position
713  (*this)[0], (*this)[1], p);
714  if (lpos == -1) {
715  return;
716  }
717  Position np = positionAtOffset(lpos);
718  if (np != *(begin())) {
719  erase(begin());
720  if (np != *(begin())) {
721  insert(begin(), p);
722  assert(size() > 1);
723  assert(*(begin()) != *(end() - 1));
724  }
725  }
726 }
727 
728 
729 void
731  // find minimum distance (from the end)
732  size_t pos = 0;
733  SUMOReal dist = 1000000;
734  size_t currPos = 0;
736  *(end() - 1),
737  GeomHelper::extrapolate_second(*(end() - 2), *(end() - 1), 100));
738 // assert(currDist>=0);
739  if (currDist >= 0 && currDist < dist) {
740  dist = currDist;
741  pos = currPos;
742  }
743 
744  for (reverse_iterator i = rbegin(); i != rend() - 1; i++, currPos++) {
745  SUMOReal currDist = GeomHelper::distancePointLine(p, *(i), *(i + 1));
746  if (currDist >= 0 && currDist < dist) {
747  dist = currDist;
748  pos = currPos;
749  }
750  }
751  // remove trailing items
752  for (size_t j = 0; j < pos; j++) {
753  erase(end() - 1);
754  }
755  // replace last item by the new position
756  SUMOReal lpos =
758  (*this)[static_cast<int>(size()) - 1], (*this)[static_cast<int>(size()) - 2], p);
759  if (lpos == -1) {
760  return;
761  }
763  length() - lpos);
764  if (np != *(end() - 1)) {
765  erase(end() - 1);
766  if (np != *(end() - 1)) {
767  push_back(np);
768  assert(size() > 1);
769  assert(*(begin()) != *(end() - 1));
770  }
771  }
772 }
773 
774 
775 SUMOReal
777  Line tmp(front(), back());
778  return tmp.atan2Angle();
779 }
780 
781 
782 void
784  if (i >= 0) {
785  erase(begin() + i);
786  } else {
787  erase(end() + i);
788  }
789 }
790 
791 
792 SUMOReal
793 PositionVector::nearest_offset_to_point2D(const Position& p, bool perpendicular) const {
795  SUMOReal nearestPos = -1;
796  SUMOReal seen = 0;
797  for (const_iterator i = begin(); i != end() - 1; i++) {
798  const SUMOReal pos =
799  GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, perpendicular);
800  const SUMOReal dist = pos < 0 ? minDist : p.distanceTo2D(Line(*i, *(i + 1)).getPositionAtDistance(pos));
801  if (dist < minDist) {
802  nearestPos = pos + seen;
803  minDist = dist;
804  }
805  if (perpendicular && i != begin()) {
806  // even if perpendicular is set we still need to check the distance to the inner points
807  const SUMOReal cornerDist = p.distanceTo2D(*i);
808  if (cornerDist < minDist) {
809  nearestPos = seen;
810  minDist = cornerDist;
811  }
812  }
813  seen += (*i).distanceTo2D(*(i + 1));
814  }
815  return nearestPos;
816 }
817 
818 
819 int
821  assert(size() > 0);
823  SUMOReal dist;
824  int closest = 0;
825  for (int i = 0; i < (int)size(); i++) {
826  dist = p.distanceTo((*this)[i]);
827  if (dist < minDist) {
828  closest = i;
829  minDist = dist;
830  }
831  }
832  return closest;
833 }
834 
835 
836 int
838  Position outIntersection = Position();
840  SUMOReal dist;
841  int insertionIndex = 1;
842  for (int i = 0; i < (int)size() - 1; i++) {
843  dist = GeomHelper::closestDistancePointLine(p, (*this)[i], (*this)[i + 1], outIntersection);
844  if (dist < minDist) {
845  insertionIndex = i + 1;
846  minDist = dist;
847  }
848  }
849  insertAt(insertionIndex, p);
850  return insertionIndex;
851 }
852 
853 
854 SUMOReal
856  if (size() == 1) {
857  return front().distanceTo(p);
858  }
859  Position outIntersection;
861  for (const_iterator i = begin(); i != end() - 1; i++) {
862  minDist = MIN2(minDist, GeomHelper::closestDistancePointLine(
863  p, *i, *(i + 1), outIntersection));
864  }
865  return minDist;
866 }
867 
868 
869 std::vector<SUMOReal>
871  std::vector<SUMOReal> ret;
872  for (const_iterator i = other.begin(); i != other.end() - 1; i++) {
873  std::vector<SUMOReal> atSegment = intersectsAtLengths2D(Line(*i, *(i + 1)));
874  copy(atSegment.begin(), atSegment.end(), back_inserter(ret));
875  }
876  return ret;
877 }
878 
879 
880 std::vector<SUMOReal>
882  std::vector<SUMOReal> ret;
883  SUMOReal pos = 0;
884  for (const_iterator i = begin(); i != end() - 1; i++) {
885  Line l((*i), *(i + 1));
886  if (GeomHelper::intersects(l.p1(), l.p2(), line.p1(), line.p2())) {
887  Position p =
888  GeomHelper::intersection_position2D(l.p1(), l.p2(), line.p1(), line.p2());
889  SUMOReal atLength = p.distanceTo2D(l.p1());
890  ret.push_back(atLength + pos);
891  }
892  pos += l.length2D();
893  }
894  return ret;
895 }
896 
897 
898 void
900  assert(size() > 1);
901  Position nb =
902  GeomHelper::extrapolate_first((*this)[0], (*this)[1], val);
903  Position ne =
905  (*this)[static_cast<int>(size()) - 2], (*this)[static_cast<int>(size()) - 1], val);
906  erase(begin());
907  push_front(nb);
908  erase(end() - 1);
909  push_back(ne);
910 }
911 
912 
915  PositionVector ret;
916  for (const_reverse_iterator i = rbegin(); i != rend(); i++) {
917  ret.push_back(*i);
918  }
919  return ret;
920 }
921 
922 
923 void
925  if (size() < 2) {
926  return;
927  }
928  PositionVector shape;
929  for (int i = 0; i < static_cast<int>(size()); i++) {
930  if (i == 0) {
931  Position from = (*this)[i];
932  Position to = (*this)[i + 1];
933  std::pair<SUMOReal, SUMOReal> offsets =
934  GeomHelper::getNormal90D_CW(from, to, amount);
935  shape.push_back(Position(from.x() - offsets.first,
936  from.y() - offsets.second, from.z()));
937  } else if (i == static_cast<int>(size()) - 1) {
938  Position from = (*this)[i - 1];
939  Position to = (*this)[i];
940  std::pair<SUMOReal, SUMOReal> offsets =
941  GeomHelper::getNormal90D_CW(from, to, amount);
942  shape.push_back(Position(to.x() - offsets.first,
943  to.y() - offsets.second, to.z()));
944  } else {
945  Position from = (*this)[i - 1];
946  Position me = (*this)[i];
947  Position to = (*this)[i + 1];
948  const double sinAngle = sin(GeomHelper::Angle2D(from.x() - me.x(), from.y() - me.y(),
949  me.x() - to.x(), me.y() - to.y()) / 2);
950  const double maxDev = 2 * (from.distanceTo2D(me) + me.distanceTo2D(to)) * sinAngle;
951  if (fabs(maxDev) < POSITION_EPS) {
952  // parallel case, just shift the middle point
953  std::pair<SUMOReal, SUMOReal> off =
954  GeomHelper::getNormal90D_CW(from, to, amount);
955  shape.push_back(Position(me.x() - off.first, me.y() - off.second, me.z()));
956  continue;
957  }
958  std::pair<SUMOReal, SUMOReal> offsets =
959  GeomHelper::getNormal90D_CW(from, me, amount);
960  std::pair<SUMOReal, SUMOReal> offsets2 =
961  GeomHelper::getNormal90D_CW(me, to, amount);
962  Line l1(
963  Position(from.x() - offsets.first, from.y() - offsets.second),
964  Position(me.x() - offsets.first, me.y() - offsets.second));
965  l1.extrapolateBy(100);
966  Line l2(
967  Position(me.x() - offsets2.first, me.y() - offsets2.second),
968  Position(to.x() - offsets2.first, to.y() - offsets2.second));
969  l2.extrapolateBy(100);
970  if (l1.intersects(l2)) {
971  shape.push_back(l1.intersectsAt(l2));
972  } else {
973  throw InvalidArgument("no line intersection");
974  }
975  }
976  }
977 
978  /*
979  ContType newCont;
980  std::pair<SUMOReal, SUMOReal> p;
981  Position newPos;
982  // first point
983  newPos = (*(begin()));
984  p = GeomHelper::getNormal90D_CW(*(begin()), *(begin()+1), amount);
985  newPos.add(p.first, p.second);
986  newCont.push_back(newPos);
987  // middle points
988  for(const_iterator i=begin()+1; i!=end()-1; i++) {
989  std::pair<SUMOReal, SUMOReal> oldp = p;
990  newPos = *i;
991  newPos.add(p.first, p.second);
992  newCont.push_back(newPos);
993  p = GeomHelper::getNormal90D_CW(*i, *(i+1), amount);
994  // Position newPos(*i);
995  // newPos.add((p.first+oldp.first)/2.0, (p.second+oldp.second)/2.0);
996  // newCont.push_back(newPos);
997  }
998  // last point
999  newPos = (*(end()-1));
1000  newPos.add(p.first, p.second);
1001  newCont.push_back(newPos);
1002  myCont = newCont;
1003  */
1004  *this = shape;
1005 }
1006 
1007 
1008 Line
1009 PositionVector::lineAt(int pos) const {
1010  assert((int)size() > pos + 1);
1011  return Line((*this)[pos], (*this)[pos + 1]);
1012 }
1013 
1014 
1015 Line
1017  return lineAt(0);
1018 }
1019 
1020 
1021 Line
1023  return lineAt((int)size() - 2);
1024 }
1025 
1026 
1027 void
1029  if ((*this)[0] == back()) {
1030  return;
1031  }
1032  push_back((*this)[0]);
1033 }
1034 
1035 
1036 std::vector<SUMOReal>
1038  std::vector<SUMOReal> ret;
1039  const_iterator i;
1040  for (i = begin(); i != end(); i++) {
1041  ret.push_back(s.distance(*i));
1042  }
1043  for (i = s.begin(); i != s.end(); i++) {
1044  ret.push_back(distance(*i));
1045  }
1046  return ret;
1047 }
1048 
1049 
1050 void
1051 PositionVector::insertAt(int index, const Position& p) {
1052  if (index >= 0) {
1053  insert(begin() + index, p);
1054  } else {
1055  insert(end() + index, p);
1056  }
1057 }
1058 
1059 
1060 void
1061 PositionVector::replaceAt(int index, const Position& p) {
1062  assert(index < static_cast<int>(size()));
1063  assert(index + static_cast<int>(size()) >= 0);
1064  if (index >= 0) {
1065  (*this)[index] = p;
1066  } else {
1067  (*this)[index + static_cast<int>(size())] = p;
1068  }
1069 }
1070 
1071 
1072 void
1074  if (size() == 0 || !p.almostSame(back())) {
1075  push_back(p);
1076  }
1077 }
1078 
1079 
1080 void
1082  if (size() == 0 || !p.almostSame(front())) {
1083  insert(begin(), p);
1084  }
1085 }
1086 
1087 
1088 bool
1090  return size() >= 2 && (*this)[0] == back();
1091 }
1092 
1093 
1094 void
1095 PositionVector::removeDoublePoints(SUMOReal minDist, bool assertLength) {
1096  if (size() > 1) {
1097  iterator last = begin();
1098  for (iterator i = begin() + 1; i != end() && (!assertLength || size() > 2);) {
1099  if (last->almostSame(*i, minDist)) {
1100  i = erase(i);
1101  } else {
1102  last = i;
1103  ++i;
1104  }
1105  }
1106  }
1107 }
1108 
1109 
1110 void
1112  if (size() > 2) {
1113  Position& last = front();
1114  for (iterator i = begin() + 1; i != end() - 1;) {
1115  if (GeomHelper::distancePointLine(*i, last, *(i + 1)) < 0.001) {
1116  i = erase(i);
1117  } else {
1118  last = *i;
1119  ++i;
1120  }
1121  }
1122  }
1123 }
1124 
1125 
1126 bool
1128  if (size() == v2.size()) {
1129  for (int i = 0; i < (int)size(); i++) {
1130  if ((*this)[i] != v2[i]) {
1131  return false;
1132  }
1133  }
1134  return true;
1135  } else {
1136  return false;
1137  }
1138 }
1139 
1140 
1141 
1142 /****************************************************************************/
1143 
SUMOReal length2D() const
Definition: Line.cpp:177
SUMOReal atan2DegreeAngle() const
Definition: Line.cpp:143
static std::pair< SUMOReal, SUMOReal > getNormal90D_CW(const Position &beg, const Position &end, SUMOReal length, SUMOReal wanted_offset)
Definition: GeomHelper.cpp:359
const Position & p2() const
Definition: Line.cpp:86
static SUMOReal Angle2D(SUMOReal x1, SUMOReal y1, SUMOReal x2, SUMOReal y2)
Definition: GeomHelper.cpp:210
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:189
SUMOReal nearest_offset_to_point2D(const Position &p, bool perpendicular=true) const
PositionVector getSubpart2D(SUMOReal beginOffset, SUMOReal endOffset) const
Position positionAtOffset(SUMOReal pos) const
Returns the position at the given length.
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:220
void insertAt(int index, const Position &p)
#define M_PI
Definition: angles.h:37
std::vector< SUMOReal > distances(const PositionVector &s) const
SUMOReal atan2DegreeSlope() const
Definition: Line.cpp:159
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
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:239
SUMOReal distanceTo(const Position &p2) const
returns the euclidean distance in 3 dimension
Definition: Position.h:219
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:213
static Position extrapolate_first(const Position &p1, const Position &p2, SUMOReal length)
Definition: GeomHelper.cpp:231
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
A class that stores a 2D geometrical boundary.
Definition: Boundary.h:48
#define WRITE_WARNING(msg)
Definition: MsgHandler.h:196
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.
static SUMOReal nearest_offset_on_line_to_point2D(const Position &l1, const Position &l2, const Position &p, bool perpendicular=true)
Definition: GeomHelper.cpp:247
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:131
void push_front_noDoublePos(const Position &p)
const Position & p1() const
Definition: Line.cpp:80
#define max(a, b)
Definition: polyfonts.c:61
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:274
SUMOReal z() const
Returns the z-position.
Definition: Position.h:73
SUMOReal distance(const Position &p) const
Definition: Line.h:51
T MIN2(T a, T b)
Definition: StdDefs.h:65
#define POSITION_EPS
Definition: config.h:186
int insertAtClosest(const Position &p)
std::string toString(const T &t, std::streamsize accuracy=OUTPUT_ACCURACY)
Definition: ToString.h:52
Position intersectsAtPoint(const Position &p1, const Position &p2) const
SUMOReal atan2Angle() const
Definition: Line.cpp:137
bool intersects(const Line &l) const
Definition: Line.cpp:171
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)
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)
Position positionAtOffset2D(SUMOReal pos) const
Returns the position at the given length.
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
SUMOReal distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
Definition: Position.h:230
void move2side(SUMOReal amount)
#define SUMOReal
Definition: config.h:215
void push_back_noDoublePos(const Position &p)
static SUMOReal closestDistancePointLine(const Position &point, const Position &lineStart, const Position &lineEnd, Position &outIntersection)
Definition: GeomHelper.cpp:298
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.
void append(const PositionVector &v)
bool crosses(const Position &p1, const Position &p2) const
Position intersectsAt(const Line &l) const
Definition: Line.cpp:165
PositionVector getSubpart(SUMOReal beginOffset, SUMOReal endOffset) const
SUMOReal isLeft(const Position &P0, const Position &P1, const Position &P2) const
std::ostream & operator<<(std::ostream &os, const PositionVector &geom)