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