SUMO - Simulation of Urban MObility
PositionVector.cpp
Go to the documentation of this file.
1 /****************************************************************************/
2 // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3 // Copyright (C) 2001-2017 German Aerospace Center (DLR) and others.
4 /****************************************************************************/
5 //
6 // This program and the accompanying materials
7 // are made available under the terms of the Eclipse Public License v2.0
8 // which accompanies this distribution, and is available at
9 // http://www.eclipse.org/legal/epl-v20.html
10 //
11 /****************************************************************************/
20 // A list of positions
21 /****************************************************************************/
22 
23 
24 // ===========================================================================
25 // included modules
26 // ===========================================================================
27 #ifdef _MSC_VER
28 #include <windows_config.h>
29 #else
30 #include <config.h>
31 #endif
32 
33 #include <queue>
34 #include <cmath>
35 #include <iostream>
36 #include <algorithm>
37 #include <cassert>
38 #include <iterator>
39 #include <limits>
40 #include <utils/common/StdDefs.h>
42 #include <utils/common/ToString.h>
43 #include "AbstractPoly.h"
44 #include "Position.h"
45 #include "PositionVector.h"
46 #include "GeomHelper.h"
47 #include "Boundary.h"
48 
49 // ===========================================================================
50 // static members
51 // ===========================================================================
53 
54 // ===========================================================================
55 // method definitions
56 // ===========================================================================
57 
59 
60 
61 PositionVector::PositionVector(const std::vector<Position>& v) {
62  std::copy(v.begin(), v.end(), std::back_inserter(*this));
63 }
64 
65 
66 PositionVector::PositionVector(const std::vector<Position>::const_iterator beg, const std::vector<Position>::const_iterator end) {
67  std::copy(beg, end, std::back_inserter(*this));
68 }
69 
70 
72  push_back(p1);
73  push_back(p2);
74 }
75 
76 
78 
79 
80 bool
81 PositionVector::around(const Position& p, double offset) const {
82  if (size() < 2) {
83  return false;
84  }
85  if (offset != 0) {
86  PositionVector tmp(*this);
87  tmp.scaleAbsolute(offset);
88  return tmp.around(p);
89  }
90  double angle = 0;
91  for (const_iterator i = begin(); i != end() - 1; i++) {
92  Position p1(
93  (*i).x() - p.x(),
94  (*i).y() - p.y());
95  Position p2(
96  (*(i + 1)).x() - p.x(),
97  (*(i + 1)).y() - p.y());
98  angle += GeomHelper::angle2D(p1, p2);
99  }
100  Position p1(
101  (*(end() - 1)).x() - p.x(),
102  (*(end() - 1)).y() - p.y());
103  Position p2(
104  (*(begin())).x() - p.x(),
105  (*(begin())).y() - p.y());
106  angle += GeomHelper::angle2D(p1, p2);
107  return (!(fabs(angle) < M_PI));
108 }
109 
110 
111 bool
112 PositionVector::overlapsWith(const AbstractPoly& poly, double offset) const {
113  if (
114  // check whether one of my points lies within the given poly
115  partialWithin(poly, offset) ||
116  // check whether the polygon lies within me
117  poly.partialWithin(*this, offset)) {
118  return true;
119  }
120  if (size() >= 2) {
121  for (const_iterator i = begin(); i != end() - 1; i++) {
122  if (poly.crosses(*i, *(i + 1))) {
123  return true;
124  }
125  }
126  if (size() > 2 && poly.crosses(back(), front())) {
127  return true;
128  }
129  }
130  return false;
131 }
132 
133 
134 double
135 PositionVector::getOverlapWith(const PositionVector& poly, double zThreshold) const {
136  double result = 0;
137  // this points within poly
138  for (const_iterator i = begin(); i != end() - 1; i++) {
139  if (poly.around(*i)) {
140  Position closest = poly.positionAtOffset2D(poly.nearest_offset_to_point2D(*i));
141  if (fabs(closest.z() - (*i).z()) < zThreshold) {
142  result = MAX2(result, poly.distance2D(*i));
143  }
144  }
145  }
146  // polys points within this
147  for (const_iterator i = poly.begin(); i != poly.end() - 1; i++) {
148  if (around(*i)) {
150  if (fabs(closest.z() - (*i).z()) < zThreshold) {
151  result = MAX2(result, distance2D(*i));
152  }
153  }
154  }
155  return result;
156 }
157 
158 
159 bool
160 PositionVector::intersects(const Position& p1, const Position& p2) const {
161  if (size() < 2) {
162  return false;
163  }
164  for (const_iterator i = begin(); i != end() - 1; i++) {
165  if (intersects(*i, *(i + 1), p1, p2)) {
166  return true;
167  }
168  }
169  return false;
170 }
171 
172 
173 bool
175  if (size() < 2) {
176  return false;
177  }
178  for (const_iterator i = begin(); i != end() - 1; i++) {
179  if (v1.intersects(*i, *(i + 1))) {
180  return true;
181  }
182  }
183  return false;
184 }
185 
186 
187 Position
188 PositionVector::intersectionPosition2D(const Position& p1, const Position& p2, const double withinDist) const {
189  for (const_iterator i = begin(); i != end() - 1; i++) {
190  double x, y, m;
191  if (intersects(*i, *(i + 1), p1, p2, withinDist, &x, &y, &m)) {
192  return Position(x, y);
193  }
194  }
195  return Position::INVALID;
196 }
197 
198 
199 Position
201  for (const_iterator i = begin(); i != end() - 1; i++) {
202  if (v1.intersects(*i, *(i + 1))) {
203  return v1.intersectionPosition2D(*i, *(i + 1));
204  }
205  }
206  return Position::INVALID;
207 }
208 
209 
210 const Position&
211 PositionVector::operator[](int index) const {
212  if (index >= 0) {
213  return at(index);
214  } else {
215  return at((int)size() + index);
216  }
217 }
218 
219 
220 Position&
222  if (index >= 0) {
223  return at(index);
224  } else {
225  return at((int)size() + index);
226  }
227 }
228 
229 
230 Position
231 PositionVector::positionAtOffset(double pos, double lateralOffset) const {
232  assert(size() != 0);
233  const_iterator i = begin();
234  double seenLength = 0;
235  do {
236  const double nextLength = (*i).distanceTo(*(i + 1));
237  if (seenLength + nextLength > pos) {
238  return positionAtOffset(*i, *(i + 1), pos - seenLength, lateralOffset);
239  }
240  seenLength += nextLength;
241  } while (++i != end() - 1);
242  if (lateralOffset == 0 || size() < 2) {
243  return back();
244  } else {
245  return positionAtOffset(*(end() - 2), *(end() - 1), (*(end() - 2)).distanceTo(*(end() - 1)), lateralOffset);
246  }
247 }
248 
249 
250 Position
251 PositionVector::positionAtOffset2D(double pos, double lateralOffset) const {
252  const_iterator i = begin();
253  double seenLength = 0;
254  do {
255  const double nextLength = (*i).distanceTo2D(*(i + 1));
256  if (seenLength + nextLength > pos) {
257  return positionAtOffset2D(*i, *(i + 1), pos - seenLength, lateralOffset);
258  }
259  seenLength += nextLength;
260  } while (++i != end() - 1);
261  return back();
262 }
263 
264 
265 double
267  if (pos < 0) {
268  pos += length();
269  }
270  const_iterator i = begin();
271  double seenLength = 0;
272  do {
273  const Position& p1 = *i;
274  const Position& p2 = *(i + 1);
275  const double nextLength = p1.distanceTo(p2);
276  if (seenLength + nextLength > pos) {
277  return p1.angleTo2D(p2);
278  }
279  seenLength += nextLength;
280  } while (++i != end() - 1);
281  const Position& p1 = (*this)[-2];
282  const Position& p2 = back();
283  return p1.angleTo2D(p2);
284 }
285 
286 
287 double
290 }
291 
292 
293 double
295  const_iterator i = begin();
296  double seenLength = 0;
297  do {
298  const Position& p1 = *i;
299  const Position& p2 = *(i + 1);
300  const double nextLength = p1.distanceTo(p2);
301  if (seenLength + nextLength > pos) {
302  return RAD2DEG(atan2(p2.z() - p1.z(), p1.distanceTo2D(p2)));
303  }
304  seenLength += nextLength;
305  } while (++i != end() - 1);
306  const Position& p1 = (*this)[-2];
307  const Position& p2 = back();
308  return RAD2DEG(atan2(p2.z() - p1.z(), p1.distanceTo2D(p2)));
309 }
310 
311 
312 Position
313 PositionVector::positionAtOffset(const Position& p1, const Position& p2, double pos, double lateralOffset) {
314  const double dist = p1.distanceTo(p2);
315  if (pos < 0. || dist < pos) {
316  return Position::INVALID;
317  }
318  if (lateralOffset != 0) {
319  if (dist == 0.) {
320  return Position::INVALID;
321  }
322  const Position offset = sideOffset(p1, p2, -lateralOffset); // move in the same direction as Position::move2side
323  if (pos == 0.) {
324  return p1 + offset;
325  }
326  return p1 + (p2 - p1) * (pos / dist) + offset;
327  }
328  if (pos == 0.) {
329  return p1;
330  }
331  return p1 + (p2 - p1) * (pos / dist);
332 }
333 
334 
335 Position
336 PositionVector::positionAtOffset2D(const Position& p1, const Position& p2, double pos, double lateralOffset) {
337  const double dist = p1.distanceTo2D(p2);
338  if (pos < 0 || dist < pos) {
339  return Position::INVALID;
340  }
341  if (lateralOffset != 0) {
342  const Position offset = sideOffset(p1, p2, -lateralOffset); // move in the same direction as Position::move2side
343  if (pos == 0.) {
344  return p1 + offset;
345  }
346  return p1 + (p2 - p1) * (pos / dist) + offset;
347  }
348  if (pos == 0.) {
349  return p1;
350  }
351  return p1 + (p2 - p1) * (pos / dist);
352 }
353 
354 
355 Boundary
357  Boundary ret;
358  for (const_iterator i = begin(); i != end(); i++) {
359  ret.add(*i);
360  }
361  return ret;
362 }
363 
364 
365 Position
367  double x = 0;
368  double y = 0;
369  double z = 0;
370  for (const_iterator i = begin(); i != end(); i++) {
371  x += (*i).x();
372  y += (*i).y();
373  z += (*i).z();
374  }
375  return Position(x / (double) size(), y / (double) size(), z / (double)size());
376 }
377 
378 
379 Position
381  PositionVector tmp = *this;
382  if (!isClosed()) { // make sure its closed
383  tmp.push_back(tmp[0]);
384  }
385  const int endIndex = (int)tmp.size() - 1;
386  double div = 0; // 6 * area including sign
387  double x = 0;
388  double y = 0;
389  if (tmp.area() != 0) { // numerical instability ?
390  // http://en.wikipedia.org/wiki/Polygon
391  for (int i = 0; i < endIndex; i++) {
392  const double z = tmp[i].x() * tmp[i + 1].y() - tmp[i + 1].x() * tmp[i].y();
393  div += z; // area formula
394  x += (tmp[i].x() + tmp[i + 1].x()) * z;
395  y += (tmp[i].y() + tmp[i + 1].y()) * z;
396  }
397  div *= 3; // 6 / 2, the 2 comes from the area formula
398  return Position(x / div, y / div);
399  } else {
400  // compute by decomposing into line segments
401  // http://en.wikipedia.org/wiki/Centroid#By_geometric_decomposition
402  double lengthSum = 0;
403  for (int i = 0; i < endIndex; i++) {
404  double length = tmp[i].distanceTo(tmp[i + 1]);
405  x += (tmp[i].x() + tmp[i + 1].x()) * length / 2;
406  y += (tmp[i].y() + tmp[i + 1].y()) * length / 2;
407  lengthSum += length;
408  }
409  if (lengthSum == 0) {
410  // it is probably only one point
411  return tmp[0];
412  }
413  return Position(x / lengthSum, y / lengthSum);
414  }
415 }
416 
417 
418 void
420  Position centroid = getCentroid();
421  for (int i = 0; i < static_cast<int>(size()); i++) {
422  (*this)[i] = centroid + (((*this)[i] - centroid) * factor);
423  }
424 }
425 
426 
427 void
429  Position centroid = getCentroid();
430  for (int i = 0; i < static_cast<int>(size()); i++) {
431  (*this)[i] = centroid + (((*this)[i] - centroid) + offset);
432  }
433 }
434 
435 
436 Position
438  if (size() == 1) {
439  return (*this)[0];
440  }
441  return positionAtOffset(double((length() / 2.)));
442 }
443 
444 
445 double
447  double len = 0;
448  for (const_iterator i = begin(); i != end() - 1; i++) {
449  len += (*i).distanceTo(*(i + 1));
450  }
451  return len;
452 }
453 
454 
455 double
457  double len = 0;
458  for (const_iterator i = begin(); i != end() - 1; i++) {
459  len += (*i).distanceTo2D(*(i + 1));
460  }
461  return len;
462 }
463 
464 
465 double
467  if (size() < 3) {
468  return 0;
469  }
470  double area = 0;
471  PositionVector tmp = *this;
472  if (!isClosed()) { // make sure its closed
473  tmp.push_back(tmp[0]);
474  }
475  const int endIndex = (int)tmp.size() - 1;
476  // http://en.wikipedia.org/wiki/Polygon
477  for (int i = 0; i < endIndex; i++) {
478  area += tmp[i].x() * tmp[i + 1].y() - tmp[i + 1].x() * tmp[i].y();
479  }
480  if (area < 0) { // we whether we had cw or ccw order
481  area *= -1;
482  }
483  return area / 2;
484 }
485 
486 
487 bool
488 PositionVector::partialWithin(const AbstractPoly& poly, double offset) const {
489  if (size() < 2) {
490  return false;
491  }
492  for (const_iterator i = begin(); i != end() - 1; i++) {
493  if (poly.around(*i, offset)) {
494  return true;
495  }
496  }
497  return false;
498 }
499 
500 
501 bool
502 PositionVector::crosses(const Position& p1, const Position& p2) const {
503  return intersects(p1, p2);
504 }
505 
506 
507 std::pair<PositionVector, PositionVector>
508 PositionVector::splitAt(double where) const {
509  if (size() < 2) {
510  throw InvalidArgument("Vector to short for splitting");
511  }
512  if (where <= POSITION_EPS || where >= length() - POSITION_EPS) {
513  WRITE_WARNING("Splitting vector close to end (pos: " + toString(where) + ", length: " + toString(length()) + ")");
514  }
515  PositionVector first, second;
516  first.push_back((*this)[0]);
517  double seen = 0;
518  const_iterator it = begin() + 1;
519  double next = first.back().distanceTo(*it);
520  // see how many points we can add to first
521  while (where >= seen + next + POSITION_EPS) {
522  seen += next;
523  first.push_back(*it);
524  it++;
525  next = first.back().distanceTo(*it);
526  }
527  if (fabs(where - (seen + next)) > POSITION_EPS || it == end() - 1) {
528  // we need to insert a new point because 'where' is not close to an
529  // existing point or it is to close to the endpoint
530  const Position p = positionAtOffset(first.back(), *it, where - seen);
531  first.push_back(p);
532  second.push_back(p);
533  } else {
534  first.push_back(*it);
535  }
536  // add the remaining points to second
537  for (; it != end(); it++) {
538  second.push_back(*it);
539  }
540  assert(first.size() >= 2);
541  assert(second.size() >= 2);
542  assert(first.back() == second.front());
543  assert(fabs(first.length() + second.length() - length()) < 2 * POSITION_EPS);
544  return std::pair<PositionVector, PositionVector>(first, second);
545 }
546 
547 
548 std::ostream&
549 operator<<(std::ostream& os, const PositionVector& geom) {
550  for (PositionVector::const_iterator i = geom.begin(); i != geom.end(); i++) {
551  if (i != geom.begin()) {
552  os << " ";
553  }
554  os << (*i);
555  }
556  return os;
557 }
558 
559 
560 void
562  std::sort(begin(), end(), as_poly_cw_sorter());
563 }
564 
565 
566 void
567 PositionVector::add(double xoff, double yoff, double zoff) {
568  for (int i = 0; i < static_cast<int>(size()); i++) {
569  (*this)[i].add(xoff, yoff, zoff);
570  }
571 }
572 
573 
574 void
576  add(offset.x(), offset.y(), offset.z());
577 }
578 
579 
580 void
582  for (int i = 0; i < static_cast<int>(size()); i++) {
583  (*this)[i].mul(1, -1);
584  }
585 }
586 
587 
589 
590 
591 int
593  return atan2(p1.x(), p1.y()) < atan2(p2.x(), p2.y());
594 }
595 
596 
597 void
599  std::sort(begin(), end(), increasing_x_y_sorter());
600 }
601 
602 
604 
605 
606 int
608  const Position& p2) const {
609  if (p1.x() != p2.x()) {
610  return p1.x() < p2.x();
611  }
612  return p1.y() < p2.y();
613 }
614 
615 
616 double
617 PositionVector::isLeft(const Position& P0, const Position& P1, const Position& P2) const {
618  return (P1.x() - P0.x()) * (P2.y() - P0.y()) - (P2.x() - P0.x()) * (P1.y() - P0.y());
619 }
620 
621 
622 void
623 PositionVector::append(const PositionVector& v, double sameThreshold) {
624  if (size() > 0 && v.size() > 0 && back().distanceTo(v[0]) < sameThreshold) {
625  copy(v.begin() + 1, v.end(), back_inserter(*this));
626  } else {
627  copy(v.begin(), v.end(), back_inserter(*this));
628  }
629 }
630 
631 
633 PositionVector::getSubpart(double beginOffset, double endOffset) const {
634  PositionVector ret;
635  Position begPos = front();
636  if (beginOffset > POSITION_EPS) {
637  begPos = positionAtOffset(beginOffset);
638  }
639  Position endPos = back();
640  if (endOffset < length() - POSITION_EPS) {
641  endPos = positionAtOffset(endOffset);
642  }
643  ret.push_back(begPos);
644 
645  double seen = 0;
646  const_iterator i = begin();
647  // skip previous segments
648  while ((i + 1) != end()
649  &&
650  seen + (*i).distanceTo(*(i + 1)) < beginOffset) {
651  seen += (*i).distanceTo(*(i + 1));
652  i++;
653  }
654  // append segments in between
655  while ((i + 1) != end()
656  &&
657  seen + (*i).distanceTo(*(i + 1)) < endOffset) {
658 
659  ret.push_back_noDoublePos(*(i + 1));
660  seen += (*i).distanceTo(*(i + 1));
661  i++;
662  }
663  // append end
664  ret.push_back_noDoublePos(endPos);
665  if (ret.size() == 1) {
666  ret.push_back(endPos);
667  }
668  return ret;
669 }
670 
671 
673 PositionVector::getSubpart2D(double beginOffset, double endOffset) const {
674  PositionVector ret;
675  Position begPos = front();
676  if (beginOffset > POSITION_EPS) {
677  begPos = positionAtOffset2D(beginOffset);
678  }
679  Position endPos = back();
680  if (endOffset < length2D() - POSITION_EPS) {
681  endPos = positionAtOffset2D(endOffset);
682  }
683  ret.push_back(begPos);
684 
685  double seen = 0;
686  const_iterator i = begin();
687  // skip previous segments
688  while ((i + 1) != end()
689  &&
690  seen + (*i).distanceTo2D(*(i + 1)) < beginOffset) {
691  seen += (*i).distanceTo2D(*(i + 1));
692  i++;
693  }
694  // append segments in between
695  while ((i + 1) != end()
696  &&
697  seen + (*i).distanceTo2D(*(i + 1)) < endOffset) {
698 
699  ret.push_back_noDoublePos(*(i + 1));
700  seen += (*i).distanceTo2D(*(i + 1));
701  i++;
702  }
703  // append end
704  ret.push_back_noDoublePos(endPos);
705  if (ret.size() == 1) {
706  ret.push_back(endPos);
707  }
708  return ret;
709 }
710 
711 
713 PositionVector::getSubpartByIndex(int beginIndex, int count) const {
714  if (beginIndex < 0) {
715  beginIndex += (int)size();
716  }
717  assert(count >= 0);
718  assert(beginIndex < (int)size());
719  assert(beginIndex + count <= (int)size());
720  PositionVector result;
721  for (int i = beginIndex; i < beginIndex + count; ++i) {
722  result.push_back((*this)[i]);
723  }
724  return result;
725 }
726 
727 
728 double
730  return front().angleTo2D(back());
731 }
732 
733 
734 double
735 PositionVector::nearest_offset_to_point2D(const Position& p, bool perpendicular) const {
736  double minDist = std::numeric_limits<double>::max();
737  double nearestPos = GeomHelper::INVALID_OFFSET;
738  double seen = 0;
739  for (const_iterator i = begin(); i != end() - 1; i++) {
740  const double pos =
741  GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, perpendicular);
742  const double dist = pos == GeomHelper::INVALID_OFFSET ? minDist : p.distanceTo2D(positionAtOffset2D(*i, *(i + 1), pos));
743  if (dist < minDist) {
744  nearestPos = pos + seen;
745  minDist = dist;
746  }
747  if (perpendicular && i != begin() && pos == GeomHelper::INVALID_OFFSET) {
748  // even if perpendicular is set we still need to check the distance to the inner points
749  const double cornerDist = p.distanceTo2D(*i);
750  if (cornerDist < minDist) {
751  const double pos1 =
752  GeomHelper::nearest_offset_on_line_to_point2D(*(i - 1), *i, p, false);
753  const double pos2 =
754  GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, false);
755  if (pos1 == (*(i - 1)).distanceTo2D(*i) && pos2 == 0.) {
756  nearestPos = seen;
757  minDist = cornerDist;
758  }
759  }
760  }
761  seen += (*i).distanceTo2D(*(i + 1));
762  }
763  return nearestPos;
764 }
765 
766 
767 Position
769  // @toDo this duplicates most of the code in nearest_offset_to_point2D. It should be refactored
770  if (extend) {
771  PositionVector extended = *this;
772  const double dist = 2 * distance2D(p);
773  extended.extrapolate(dist);
774  return extended.transformToVectorCoordinates(p) - Position(dist, 0);
775  }
776  double minDist = std::numeric_limits<double>::max();
777  double nearestPos = -1;
778  double seen = 0;
779  int sign = 1;
780  for (const_iterator i = begin(); i != end() - 1; i++) {
781  const double pos =
782  GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, true);
783  const double dist = pos < 0 ? minDist : p.distanceTo2D(positionAtOffset(*i, *(i + 1), pos));
784  if (dist < minDist) {
785  nearestPos = pos + seen;
786  minDist = dist;
787  sign = isLeft(*i, *(i + 1), p) >= 0 ? -1 : 1;
788  }
789  if (i != begin() && pos == GeomHelper::INVALID_OFFSET) {
790  // even if perpendicular is set we still need to check the distance to the inner points
791  const double cornerDist = p.distanceTo2D(*i);
792  if (cornerDist < minDist) {
793  const double pos1 =
794  GeomHelper::nearest_offset_on_line_to_point2D(*(i - 1), *i, p, false);
795  const double pos2 =
796  GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, false);
797  if (pos1 == (*(i - 1)).distanceTo2D(*i) && pos2 == 0.) {
798  nearestPos = seen;
799  minDist = cornerDist;
800  sign = isLeft(*(i - 1), *i, p) >= 0 ? -1 : 1;
801  }
802  }
803  }
804  seen += (*i).distanceTo2D(*(i + 1));
805  }
806  if (nearestPos != -1) {
807  return Position(nearestPos, sign * minDist);
808  } else {
809  return Position::INVALID;
810  }
811 }
812 
813 
814 int
816  assert(size() > 0);
817  double minDist = std::numeric_limits<double>::max();
818  double dist;
819  int closest = 0;
820  for (int i = 0; i < (int)size(); i++) {
821  dist = p.distanceTo((*this)[i]);
822  if (dist < minDist) {
823  closest = i;
824  minDist = dist;
825  }
826  }
827  return closest;
828 }
829 
830 
831 int
833  double minDist = std::numeric_limits<double>::max();
834  int insertionIndex = 1;
835  for (int i = 0; i < (int)size() - 1; i++) {
836  const double length = GeomHelper::nearest_offset_on_line_to_point2D((*this)[i], (*this)[i + 1], p, false);
837  const Position& outIntersection = PositionVector::positionAtOffset2D((*this)[i], (*this)[i + 1], length);
838  const double dist = p.distanceTo2D(outIntersection);
839  if (dist < minDist) {
840  insertionIndex = i + 1;
841  minDist = dist;
842  }
843  }
844  insert(begin() + insertionIndex, p);
845  return insertionIndex;
846 }
847 
848 
849 int
851  if (size() == 0) {
852  return -1;
853  }
854  double minDist = std::numeric_limits<double>::max();
855  int removalIndex = 0;
856  for (int i = 0; i < (int)size(); i++) {
857  const double dist = p.distanceTo2D((*this)[i]);
858  if (dist < minDist) {
859  removalIndex = i;
860  minDist = dist;
861  }
862  }
863  erase(begin() + removalIndex);
864  return removalIndex;
865 }
866 
867 
868 std::vector<double>
870  std::vector<double> ret;
871  for (const_iterator i = other.begin(); i != other.end() - 1; i++) {
872  std::vector<double> atSegment = intersectsAtLengths2D(*i, *(i + 1));
873  copy(atSegment.begin(), atSegment.end(), back_inserter(ret));
874  }
875  return ret;
876 }
877 
878 
879 std::vector<double>
881  std::vector<double> ret;
882  double pos = 0;
883  for (const_iterator i = begin(); i != end() - 1; i++) {
884  const Position& p1 = *i;
885  const Position& p2 = *(i + 1);
886  double x, y, m;
887  if (intersects(p1, p2, lp1, lp2, 0., &x, &y, &m)) {
888  ret.push_back(Position(x, y).distanceTo2D(p1) + pos);
889  }
890  pos += p1.distanceTo2D(p2);
891  }
892  return ret;
893 }
894 
895 
896 void
897 PositionVector::extrapolate(const double val, const bool onlyFirst, const bool onlyLast) {
898  assert(size() > 1);
899  Position& p1 = (*this)[0];
900  Position& p2 = (*this)[1];
901  const Position offset = (p2 - p1) * (val / p1.distanceTo(p2));
902  if (!onlyLast) {
903  p1.sub(offset);
904  }
905  if (!onlyFirst) {
906  if (size() == 2) {
907  p2.add(offset);
908  } else {
909  const Position& e1 = (*this)[-2];
910  Position& e2 = (*this)[-1];
911  e2.sub((e1 - e2) * (val / e1.distanceTo(e2)));
912  }
913  }
914 }
915 
916 
917 void
918 PositionVector::extrapolate2D(const double val, const bool onlyFirst) {
919  assert(size() > 1);
920  Position& p1 = (*this)[0];
921  Position& p2 = (*this)[1];
922  const Position offset = (p2 - p1) * (val / p1.distanceTo2D(p2));
923  p1.sub(offset);
924  if (!onlyFirst) {
925  if (size() == 2) {
926  p2.add(offset);
927  } else {
928  const Position& e1 = (*this)[-2];
929  Position& e2 = (*this)[-1];
930  e2.sub((e1 - e2) * (val / e1.distanceTo2D(e2)));
931  }
932  }
933 }
934 
935 
938  PositionVector ret;
939  for (const_reverse_iterator i = rbegin(); i != rend(); i++) {
940  ret.push_back(*i);
941  }
942  return ret;
943 }
944 
945 
946 Position
947 PositionVector::sideOffset(const Position& beg, const Position& end, const double amount) {
948  const double scale = amount / beg.distanceTo2D(end);
949  return Position((beg.y() - end.y()) * scale, (end.x() - beg.x()) * scale);
950 }
951 
952 
953 void
955  if (size() < 2) {
956  return;
957  }
959  if (length2D() == 0) {
960  return;
961  }
962  PositionVector shape;
963  for (int i = 0; i < static_cast<int>(size()); i++) {
964  if (i == 0) {
965  const Position& from = (*this)[i];
966  const Position& to = (*this)[i + 1];
967  if (from != to) {
968  shape.push_back(from - sideOffset(from, to, amount));
969  }
970  } else if (i == static_cast<int>(size()) - 1) {
971  const Position& from = (*this)[i - 1];
972  const Position& to = (*this)[i];
973  if (from != to) {
974  shape.push_back(to - sideOffset(from, to, amount));
975  }
976  } else {
977  const Position& from = (*this)[i - 1];
978  const Position& me = (*this)[i];
979  const Position& to = (*this)[i + 1];
980  PositionVector fromMe(from, me);
981  fromMe.extrapolate2D(me.distanceTo2D(to));
982  const double extrapolateDev = fromMe[1].distanceTo2D(to);
983  if (fabs(extrapolateDev) < POSITION_EPS) {
984  // parallel case, just shift the middle point
985  shape.push_back(me - sideOffset(from, to, amount));
986  } else if (fabs(extrapolateDev - 2 * me.distanceTo2D(to)) < POSITION_EPS) {
987  // counterparallel case, just shift the middle point
988  PositionVector fromMe(from, me);
989  fromMe.extrapolate2D(amount);
990  shape.push_back(fromMe[1]);
991  } else {
992  Position offsets = sideOffset(from, me, amount);
993  Position offsets2 = sideOffset(me, to, amount);
994  PositionVector l1(from - offsets, me - offsets);
995  PositionVector l2(me - offsets2, to - offsets2);
996  Position meNew = l1.intersectionPosition2D(l2[0], l2[1], 100);
997  if (meNew == Position::INVALID) {
998  throw InvalidArgument("no line intersection");
999  }
1000  meNew = meNew + Position(0, 0, me.z());
1001  shape.push_back(meNew);
1002  }
1003  // copy original z value
1004  shape.back().set(shape.back().x(), shape.back().y(), me.z());
1005  }
1006  }
1007  *this = shape;
1008 }
1009 
1010 
1011 double
1013  assert((int)size() > pos + 1);
1014  return (*this)[pos].angleTo2D((*this)[pos + 1]);
1015 }
1016 
1017 
1018 void
1020  if (size() == 0 || (*this)[0] == back()) {
1021  return;
1022  }
1023  push_back((*this)[0]);
1024 }
1025 
1026 
1027 std::vector<double>
1028 PositionVector::distances(const PositionVector& s, bool perpendicular) const {
1029  std::vector<double> ret;
1030  const_iterator i;
1031  for (i = begin(); i != end(); i++) {
1032  const double dist = s.distance2D(*i, perpendicular);
1033  if (dist != GeomHelper::INVALID_OFFSET) {
1034  ret.push_back(dist);
1035  }
1036  }
1037  for (i = s.begin(); i != s.end(); i++) {
1038  const double dist = distance2D(*i, perpendicular);
1039  if (dist != GeomHelper::INVALID_OFFSET) {
1040  ret.push_back(dist);
1041  }
1042  }
1043  return ret;
1044 }
1045 
1046 
1047 double
1048 PositionVector::distance2D(const Position& p, bool perpendicular) const {
1049  if (size() == 0) {
1050  return std::numeric_limits<double>::max();
1051  } else if (size() == 1) {
1052  return front().distanceTo(p);
1053  }
1054  const double nearestOffset = nearest_offset_to_point2D(p, perpendicular);
1055  if (nearestOffset == GeomHelper::INVALID_OFFSET) {
1057  } else {
1058  return p.distanceTo2D(positionAtOffset2D(nearestOffset));
1059  }
1060 }
1061 
1062 
1063 void
1065  if (size() == 0 || !p.almostSame(back())) {
1066  push_back(p);
1067  }
1068 }
1069 
1070 
1071 void
1073  if (size() == 0 || !p.almostSame(front())) {
1074  insert(begin(), p);
1075  }
1076 }
1077 
1078 
1079 void
1080 PositionVector::insert_noDoublePos(const std::vector<Position>::iterator& at, const Position& p) {
1081  if (at == begin()) {
1083  } else if (at == end()) {
1085  } else {
1086  if (!p.almostSame(*at) && !p.almostSame(*(at - 1))) {
1087  insert(at, p);
1088  }
1089  }
1090 }
1091 
1092 
1093 bool
1095  return size() >= 2 && (*this)[0] == back();
1096 }
1097 
1098 
1099 void
1100 PositionVector::removeDoublePoints(double minDist, bool assertLength) {
1101  if (size() > 1) {
1102  iterator last = begin();
1103  for (iterator i = begin() + 1; i != end() && (!assertLength || size() > 2);) {
1104  if (last->almostSame(*i, minDist)) {
1105  i = erase(i);
1106  } else {
1107  last = i;
1108  ++i;
1109  }
1110  }
1111  }
1112 }
1113 
1114 
1115 bool
1117  return static_cast<vp>(*this) == static_cast<vp>(v2);
1118 }
1119 
1120 
1121 bool
1123  return static_cast<vp>(*this) != static_cast<vp>(v2);
1124 }
1125 
1128  if (length() != v2.length()) {
1129  WRITE_ERROR("Trying to substract PositionVectors of different lengths.");
1130  }
1131  PositionVector pv;
1132  auto i1 = begin();
1133  auto i2 = v2.begin();
1134  while (i1 != end()) {
1135  pv.add(*i1 - *i2);
1136  }
1137  return pv;
1138 }
1139 
1142  if (length() != v2.length()) {
1143  WRITE_ERROR("Trying to substract PositionVectors of different lengths.");
1144  }
1145  PositionVector pv;
1146  auto i1 = begin();
1147  auto i2 = v2.begin();
1148  while (i1 != end()) {
1149  pv.add(*i1 + *i2);
1150  }
1151  return pv;
1152 }
1153 
1154 bool
1156  if (size() < 2) {
1157  return false;
1158  }
1159  for (const_iterator i = begin(); i != end() - 1; i++) {
1160  if ((*i).z() != (*(i + 1)).z()) {
1161  return true;
1162  }
1163  }
1164  return false;
1165 }
1166 
1167 
1168 bool
1169 PositionVector::intersects(const Position& p11, const Position& p12, const Position& p21, const Position& p22, const double withinDist, double* x, double* y, double* mu) {
1170  const double eps = std::numeric_limits<double>::epsilon();
1171  const double denominator = (p22.y() - p21.y()) * (p12.x() - p11.x()) - (p22.x() - p21.x()) * (p12.y() - p11.y());
1172  const double numera = (p22.x() - p21.x()) * (p11.y() - p21.y()) - (p22.y() - p21.y()) * (p11.x() - p21.x());
1173  const double numerb = (p12.x() - p11.x()) * (p11.y() - p21.y()) - (p12.y() - p11.y()) * (p11.x() - p21.x());
1174  /* Are the lines coincident? */
1175  if (fabs(numera) < eps && fabs(numerb) < eps && fabs(denominator) < eps) {
1176  double a1;
1177  double a2;
1178  double a3;
1179  double a4;
1180  double a = -1e12;
1181  if (p11.x() != p12.x()) {
1182  a1 = p11.x() < p12.x() ? p11.x() : p12.x();
1183  a2 = p11.x() < p12.x() ? p12.x() : p11.x();
1184  a3 = p21.x() < p22.x() ? p21.x() : p22.x();
1185  a4 = p21.x() < p22.x() ? p22.x() : p21.x();
1186  } else {
1187  a1 = p11.y() < p12.y() ? p11.y() : p12.y();
1188  a2 = p11.y() < p12.y() ? p12.y() : p11.y();
1189  a3 = p21.y() < p22.y() ? p21.y() : p22.y();
1190  a4 = p21.y() < p22.y() ? p22.y() : p21.y();
1191  }
1192  if (a1 <= a3 && a3 <= a2) {
1193  if (a4 < a2) {
1194  a = (a3 + a4) / 2;
1195  } else {
1196  a = (a2 + a3) / 2;
1197  }
1198  }
1199  if (a3 <= a1 && a1 <= a4) {
1200  if (a2 < a4) {
1201  a = (a1 + a2) / 2;
1202  } else {
1203  a = (a1 + a4) / 2;
1204  }
1205  }
1206  if (a != -1e12) {
1207  if (x != 0) {
1208  if (p11.x() != p12.x()) {
1209  *mu = (a - p11.x()) / (p12.x() - p11.x());
1210  *x = a;
1211  *y = p11.y() + (*mu) * (p12.y() - p11.y());
1212  } else {
1213  *x = p11.x();
1214  *y = a;
1215  if (p12.y() == p11.y()) {
1216  *mu = 0;
1217  } else {
1218  *mu = (a - p11.y()) / (p12.y() - p11.y());
1219  }
1220  }
1221  }
1222  return true;
1223  }
1224  return false;
1225  }
1226  /* Are the lines parallel */
1227  if (fabs(denominator) < eps) {
1228  return false;
1229  }
1230  /* Is the intersection along the segments */
1231  double mua = numera / denominator;
1232  /* reduce rounding errors for lines ending in the same point */
1233  if (fabs(p12.x() - p22.x()) < eps && fabs(p12.y() - p22.y()) < eps) {
1234  mua = 1.;
1235  } else {
1236  const double offseta = withinDist / p11.distanceTo2D(p12);
1237  const double offsetb = withinDist / p21.distanceTo2D(p22);
1238  const double mub = numerb / denominator;
1239  if (mua < -offseta || mua > 1 + offseta || mub < -offsetb || mub > 1 + offsetb) {
1240  return false;
1241  }
1242  }
1243  if (x != 0) {
1244  *x = p11.x() + mua * (p12.x() - p11.x());
1245  *y = p11.y() + mua * (p12.y() - p11.y());
1246  *mu = mua;
1247  }
1248  return true;
1249 }
1250 
1251 
1252 void
1254  const double s = sin(angle);
1255  const double c = cos(angle);
1256  for (int i = 0; i < (int)size(); i++) {
1257  const double x = (*this)[i].x();
1258  const double y = (*this)[i].y();
1259  const double z = (*this)[i].z();
1260  const double xnew = x * c - y * s;
1261  const double ynew = x * s + y * c;
1262  (*this)[i].set(xnew, ynew, z);
1263  }
1264 }
1265 
1266 
1269  PositionVector result = *this;
1270  bool changed = true;
1271  while (changed && result.size() > 3) {
1272  changed = false;
1273  for (int i = 0; i < (int)result.size(); i++) {
1274  const Position& p1 = result[i];
1275  const Position& p2 = result[(i + 2) % result.size()];
1276  const int middleIndex = (i + 1) % result.size();
1277  const Position& p0 = result[middleIndex];
1278  // https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Line_defined_by_two_points
1279  const double triangleArea2 = fabs((p2.y() - p1.y()) * p0.x() - (p2.x() - p1.x()) * p0.y() + p2.x() * p1.y() - p2.y() * p1.x());
1280  const double distIK = p1.distanceTo2D(p2);
1281  if (distIK > NUMERICAL_EPS && triangleArea2 / distIK < NUMERICAL_EPS) {
1282  changed = true;
1283  result.erase(result.begin() + middleIndex);
1284  break;
1285  }
1286  }
1287  }
1288  return result;
1289 }
1290 
1291 
1293 PositionVector::getOrthogonal(const Position& p, double extend, bool before, double length) const {
1294  PositionVector result;
1295  PositionVector tmp = *this;
1296  tmp.extrapolate2D(extend);
1297  const double baseOffset = tmp.nearest_offset_to_point2D(p);
1298  if (baseOffset == GeomHelper::INVALID_OFFSET || size() < 2) {
1299  // fail
1300  return result;
1301  }
1302  Position base = tmp.positionAtOffset2D(baseOffset);
1303  const int closestIndex = tmp.indexOfClosest(base);
1304  result.push_back(base);
1305  if (fabs(baseOffset - tmp.offsetAtIndex2D(closestIndex)) > NUMERICAL_EPS) {
1306  result.push_back(tmp[closestIndex]);
1307  } else if (before) {
1308  // take the segment before closestIndex if possible
1309  if (closestIndex > 0) {
1310  result.push_back(tmp[closestIndex - 1]);
1311  } else {
1312  result.push_back(tmp[1]);
1313  }
1314  } else {
1315  // take the segment after closestIndex if possible
1316  if (closestIndex < (int)size() - 1) {
1317  result.push_back(tmp[closestIndex + 1]);
1318  } else {
1319  result.push_back(tmp[-1]);
1320  }
1321  }
1322  result = result.getSubpart2D(0, length);
1323  // rotate around base
1324  result.add(base * -1);
1325  result.rotate2D(DEG2RAD(90));
1326  result.add(base);
1327  return result;
1328 }
1329 
1330 
1333  PositionVector result = *this;
1334  const double z0 = (*this)[0].z();
1335  // the z-delta of the first segment
1336  const double dz = (*this)[1].z() - z0;
1337  // if the shape only has 2 points it is as smooth as possible already
1338  if (size() > 2 && dz != 0) {
1339  dist = MIN2(dist, length());
1340  // check wether we need to insert a new point at dist
1341  Position pDist = positionAtOffset(dist);
1342  int iLast = indexOfClosest(pDist);
1343  // prevent close spacing to reduce impact of rounding errors in z-axis
1344  if (pDist.distanceTo2D((*this)[iLast]) > POSITION_EPS * 20) {
1345  iLast = result.insertAtClosest(pDist);
1346  }
1347  double dist2 = result.offsetAtIndex2D(iLast);
1348  const double dz2 = result[iLast].z() - z0;
1349  double seen = 0;
1350  for (int i = 1; i < iLast; ++i) {
1351  seen += result[i].distanceTo2D(result[i - 1]);
1352  result[i].set(result[i].x(), result[i].y(), z0 + dz2 * seen / dist2);
1353  }
1354  }
1355  return result;
1356 
1357 }
1358 
1359 
1361 PositionVector::interpolateZ(double zStart, double zEnd) const {
1362  PositionVector result = *this;
1363  if (size() == 0) {
1364  return result;
1365  }
1366  result[0].setz(zStart);
1367  result[-1].setz(zEnd);
1368  const double dz = zEnd - zStart;
1369  const double length = length2D();
1370  double seen = 0;
1371  for (int i = 1; i < (int)size() - 1; ++i) {
1372  seen += result[i].distanceTo2D(result[i - 1]);
1373  result[i].setz(zStart + dz * seen / length);
1374  }
1375  return result;
1376 }
1377 
1379 PositionVector::resample(double maxLength) const {
1380  PositionVector result;
1381  const double length = length2D();
1382  if (length < POSITION_EPS) {
1383  return result;
1384  }
1385  maxLength = length / ceil(length / maxLength);
1386  for (double pos = 0; pos <= length; pos += maxLength) {
1387  result.push_back(positionAtOffset2D(pos));
1388  }
1389  return result;
1390 }
1391 
1392 double
1394  if (index < 0 || index >= (int)size()) {
1396  }
1397  double seen = 0;
1398  for (int i = 1; i <= index; ++i) {
1399  seen += (*this)[i].distanceTo2D((*this)[i - 1]);
1400  }
1401  return seen;
1402 }
1403 
1404 
1405 double
1407  double result = 0;
1408  for (int i = 1; i < (int)size(); ++i) {
1409  const Position& p1 = (*this)[i - 1];
1410  const Position& p2 = (*this)[i];
1411  result = MAX2(result, (double)fabs((p1.z() - p2.z()) / p1.distanceTo2D(p2)));
1412  }
1413  return result;
1414 }
1415 
1416 /****************************************************************************/
1417 
PositionVector operator+(const PositionVector &v2) const
adds two vectors (requires vectors of the same length)
bool around(const Position &p, double offset=0) const
Returns the information whether the position vector describes a polygon lying around the given point...
static const PositionVector EMPTY
empty Vector
clase for increasing Sorter
double rotationDegreeAtOffset(double pos) const
Returns the rotation at the given length.
virtual bool partialWithin(const AbstractPoly &poly, double offset=0) const =0
int operator()(const Position &p1, const Position &p2) const
comparing operation
bool almostSame(const Position &p2, double maxDiv=POSITION_EPS) const
checki if two position is almost the sme as other
Definition: Position.h:234
double length2D() const
Returns the length.
void append(const PositionVector &v, double sameThreshold=2.0)
PositionVector getOrthogonal(const Position &p, double extend, bool before, double length=1.0) const
return orthogonal through p (extending this vector if necessary)
double z() const
Returns the z-position.
Definition: Position.h:72
void sortAsPolyCWByAngle()
short as polygon CV by angle
double 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) ...
void add(const Position &pos)
Adds the given position to this one.
Definition: Position.h:132
int indexOfClosest(const Position &p) const
index of the closest position to p
double distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
Definition: Position.h:249
friend std::ostream & operator<<(std::ostream &os, const PositionVector &geom)
Position positionAtOffset2D(double pos, double lateralOffset=0) const
Returns the position at the given length.
PositionVector resample(double maxLength) const
resample shape with the given number of points (equal spacing)
Position intersectionPosition2D(const Position &p1, const Position &p2, const double withinDist=0.) const
Returns the position of the intersection.
double y() const
Returns the y-position.
Definition: Position.h:67
double x() const
Returns the x-position.
Definition: Position.h:62
virtual bool crosses(const Position &p1, const Position &p2) const =0
double angleTo2D(const Position &other) const
returns the angle in the plane of the vector pointing from here to the other position ...
Definition: Position.h:259
Position getCentroid() const
Returns the centroid (closes the polygon if unclosed)
T MAX2(T a, T b)
Definition: StdDefs.h:73
bool partialWithin(const AbstractPoly &poly, double offset=0) const
Returns the information whether this polygon lies partially within the given polygon.
PositionVector reverse() const
reverse position vector
double rotationAtOffset(double pos) const
Returns the rotation at the given length.
PositionVector interpolateZ(double zStart, double zEnd) const
returned vector that varies z smoothly over its length
#define RAD2DEG(x)
Definition: GeomHelper.h:45
void insert_noDoublePos(const std::vector< Position >::iterator &at, const Position &p)
insert in front a non double position
std::vector< double > distances(const PositionVector &s, bool perpendicular=false) const
distances of all my points to s and all of s points to myself
bool hasElevation() const
return whether two positions differ in z-coordinate
std::pair< PositionVector, PositionVector > splitAt(double where) const
Returns the two lists made when this list vector is splitted at the given point.
bool operator!=(const PositionVector &v2) const
comparing operation
A class that stores a 2D geometrical boundary.
Definition: Boundary.h:47
#define WRITE_WARNING(msg)
Definition: MsgHandler.h:199
Position getLineCenter() const
get line center
static double legacyDegree(const double angle, const bool positive=false)
Definition: GeomHelper.cpp:205
double area() const
Returns the area (0 for non-closed)
~PositionVector()
Destructor.
void extrapolate2D(const double val, const bool onlyFirst=false)
extrapolate position vector in two dimensions (Z is ignored)
void push_front_noDoublePos(const Position &p)
insert in front a non double position
void removeDoublePoints(double minDist=POSITION_EPS, bool assertLength=false)
Removes positions if too near.
static double nearest_offset_on_line_to_point2D(const Position &lineStart, const Position &lineEnd, const Position &p, bool perpendicular=true)
Definition: GeomHelper.cpp:95
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:55
void scaleAbsolute(double offset)
enlarges/shrinks the polygon by an absolute offset based at the centroid
bool operator==(const PositionVector &v2) const
comparing operation
double beginEndAngle() const
returns the angle in radians of the line connecting the first and the last position ...
A point in 2D or 3D with translation and scaling methods.
Definition: Position.h:45
std::vector< Position > vp
vector of position
A list of positions.
Position getPolygonCenter() const
Returns the arithmetic of all corner points.
int operator()(const Position &p1, const Position &p2) const
comparing operation for sort
virtual bool around(const Position &p, double offset=0) const =0
static double 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:89
T MIN2(T a, T b)
Definition: StdDefs.h:67
#define POSITION_EPS
Definition: config.h:175
std::vector< double > intersectsAtLengths2D(const PositionVector &other) const
For all intersections between this vector and other, return the 2D-length of the subvector from this ...
const Position & operator[](int index) const
returns the constat position at the given index !!! exceptions?
int insertAtClosest(const Position &p)
inserts p between the two closest positions and returns the insertion index
PositionVector getSubpart(double beginOffset, double endOffset) const
get subpart of a position vector
bool overlapsWith(const AbstractPoly &poly, double offset=0) const
Returns the information whether the given polygon overlaps with this.
#define DEG2RAD(x)
Definition: GeomHelper.h:44
PositionVector smoothedZFront(double dist=std::numeric_limits< double >::max()) const
returned vector that is smoothed at the front (within dist)
static const double INVALID_OFFSET
a value to signify offsets outside the range of [0, Line.length()]
Definition: GeomHelper.h:58
PositionVector getSubpartByIndex(int beginIndex, int count) const
get subpart of a position vector using index and a cout
void sortByIncreasingXY()
shory by increasing X-Y Psitions
void move2side(double amount)
move position vector to side using certain ammount
PositionVector operator-(const PositionVector &v2) const
substracts two vectors (requires vectors of the same length)
PositionVector simplified() const
return the same shape with intermediate colinear points removed
double slopeDegreeAtOffset(double pos) const
Returns the slope at the given length.
PositionVector()
Constructor. Creates an empty position vector.
#define WRITE_ERROR(msg)
Definition: MsgHandler.h:205
double isLeft(const Position &P0, const Position &P1, const Position &P2) const
get left
PositionVector getSubpart2D(double beginOffset, double endOffset) const
get subpart of a position vector in two dimensions (Z is ignored)
void rotate2D(double angle)
void extrapolate(const double val, const bool onlyFirst=false, const bool onlyLast=false)
extrapolate position vector
double length() const
Returns the length.
#define M_PI
Definition: odrSpiral.cpp:40
bool isClosed() const
check if PositionVector is closed
static Position sideOffset(const Position &beg, const Position &end, const double amount)
get a side position of position vector using a offset
void scaleRelative(double factor)
enlarges/shrinks the polygon by a factor based at the centroid
double angleAt2D(int pos) const
get angle in certain position of position vector
double offsetAtIndex2D(int index) const
return the offset at the given index
double distanceTo(const Position &p2) const
returns the euclidean distance in 3 dimension
Definition: Position.h:239
Boundary getBoxBoundary() const
Returns a boundary enclosing this list of lines.
#define NUMERICAL_EPS
Definition: config.h:151
void push_back_noDoublePos(const Position &p)
insert in back a non double position
double getMaxGrade() const
return the maximum grade of all segments as a fraction of zRange/length2D
bool crosses(const Position &p1, const Position &p2) const
double getOverlapWith(const PositionVector &poly, double zThreshold) const
Returns the maximum overlaps between this and the given polygon (when not separated by at least zThre...
void add(double x, double y, double z=0)
Makes the boundary include the given coordinate.
Definition: Boundary.cpp:85
void add(double xoff, double yoff, double zoff)
double nearest_offset_to_point2D(const Position &p, bool perpendicular=true) const
return the nearest offest to point 2D
void closePolygon()
ensures that the last position equals the first
Position positionAtOffset(double pos, double lateralOffset=0) const
Returns the position at the given length.
int removeClosest(const Position &p)
removes the point closest to p and return the removal index
Position transformToVectorCoordinates(const Position &p, bool extend=false) const
return position p within the length-wise coordinate system defined by this position vector...
bool intersects(const Position &p1, const Position &p2) const
Returns the information whether this list of points interesects the given line.
static const Position INVALID
used to indicate that a position is valid
Definition: Position.h:277
void sub(double dx, double dy)
Substracts the given position from this one.
Definition: Position.h:152