Eclipse 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-2022 German Aerospace Center (DLR) and others.
4 // This program and the accompanying materials are made available under the
5 // terms of the Eclipse Public License 2.0 which is available at
6 // https://www.eclipse.org/legal/epl-2.0/
7 // This Source Code may also be made available under the following Secondary
8 // Licenses when the conditions for such availability set forth in the Eclipse
9 // Public License 2.0 are satisfied: GNU General Public License, version 2
10 // or later which is available at
11 // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12 // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13 /****************************************************************************/
21 // A list of positions
22 /****************************************************************************/
23 #include <config.h>
24 
25 #include <queue>
26 #include <cmath>
27 #include <iostream>
28 #include <algorithm>
29 #include <cassert>
30 #include <iterator>
31 #include <limits>
32 #include <utils/common/StdDefs.h>
34 #include <utils/common/ToString.h>
35 #include "AbstractPoly.h"
36 #include "Position.h"
37 #include "PositionVector.h"
38 #include "GeomHelper.h"
39 #include "Boundary.h"
40 
41 //#define DEBUG_MOVE2SIDE
42 
43 // ===========================================================================
44 // static members
45 // ===========================================================================
47 
48 // ===========================================================================
49 // method definitions
50 // ===========================================================================
51 
53 
54 
55 PositionVector::PositionVector(const std::vector<Position>& v) {
56  std::copy(v.begin(), v.end(), std::back_inserter(*this));
57 }
58 
59 
60 PositionVector::PositionVector(const std::vector<Position>::const_iterator beg, const std::vector<Position>::const_iterator end) {
61  std::copy(beg, end, std::back_inserter(*this));
62 }
63 
64 
66  push_back(p1);
67  push_back(p2);
68 }
69 
70 
72 
73 
74 bool
75 PositionVector::around(const Position& p, double offset) const {
76  if (size() < 2) {
77  return false;
78  }
79  if (offset != 0) {
80  PositionVector tmp(*this);
81  tmp.scaleAbsolute(offset);
82  return tmp.around(p);
83  }
84  double angle = 0;
85  // iterate over all points, and obtain angle between current and next
86  for (const_iterator i = begin(); i != (end() - 1); i++) {
87  Position p1(
88  i->x() - p.x(),
89  i->y() - p.y());
90  Position p2(
91  (i + 1)->x() - p.x(),
92  (i + 1)->y() - p.y());
93  angle += GeomHelper::angle2D(p1, p2);
94  }
95  // add angle between last and first point
96  Position p1(
97  (end() - 1)->x() - p.x(),
98  (end() - 1)->y() - p.y());
99  Position p2(
100  begin()->x() - p.x(),
101  begin()->y() - p.y());
102  angle += GeomHelper::angle2D(p1, p2);
103  // if angle is less than PI, then point lying in Polygon
104  return (!(fabs(angle) < M_PI));
105 }
106 
107 
108 bool
109 PositionVector::overlapsWith(const AbstractPoly& poly, double offset) const {
110  if (
111  // check whether one of my points lies within the given poly
112  partialWithin(poly, offset) ||
113  // check whether the polygon lies within me
114  poly.partialWithin(*this, offset)) {
115  return true;
116  }
117  if (size() >= 2) {
118  for (const_iterator i = begin(); i != end() - 1; i++) {
119  if (poly.crosses(*i, *(i + 1))) {
120  return true;
121  }
122  }
123  if (size() > 2 && poly.crosses(back(), front())) {
124  return true;
125  }
126  }
127  return false;
128 }
129 
130 
131 double
132 PositionVector::getOverlapWith(const PositionVector& poly, double zThreshold) const {
133  double result = 0;
134  if ((size() == 0) || (poly.size() == 0)) {
135  return result;
136  }
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  /* bracket operators works as in Python. Examples:
213  - A = {'a', 'b', 'c', 'd'} (size 4)
214  - A [2] returns 'c' because 0 < 2 < 4
215  - A [100] thrown an exception because 100 > 4
216  - A [-1] returns 'd' because 4 - 1 = 3
217  - A [-100] thrown an exception because (4-100) < 0
218  */
219  if (index >= 0 && index < (int)size()) {
220  return at(index);
221  } else if (index < 0 && -index <= (int)size()) {
222  return at((int)size() + index);
223  } else {
224  throw ProcessError("Index out of range in bracket operator of PositionVector");
225  }
226 }
227 
228 
229 Position&
231  /* bracket operators works as in Python. Examples:
232  - A = {'a', 'b', 'c', 'd'} (size 4)
233  - A [2] returns 'c' because 0 < 2 < 4
234  - A [100] thrown an exception because 100 > 4
235  - A [-1] returns 'd' because 4 - 1 = 3
236  - A [-100] thrown an exception because (4-100) < 0
237  */
238  if (index >= 0 && index < (int)size()) {
239  return at(index);
240  } else if (index < 0 && -index <= (int)size()) {
241  return at((int)size() + index);
242  } else {
243  throw ProcessError("Index out of range in bracket operator of PositionVector");
244  }
245 }
246 
247 
248 Position
249 PositionVector::positionAtOffset(double pos, double lateralOffset) const {
250  if (size() == 0) {
251  return Position::INVALID;
252  }
253  if (size() == 1) {
254  return front();
255  }
256  const_iterator i = begin();
257  double seenLength = 0;
258  do {
259  const double nextLength = (*i).distanceTo(*(i + 1));
260  if (seenLength + nextLength > pos) {
261  return positionAtOffset(*i, *(i + 1), pos - seenLength, lateralOffset);
262  }
263  seenLength += nextLength;
264  } while (++i != end() - 1);
265  if (lateralOffset == 0 || size() < 2) {
266  return back();
267  } else {
268  return positionAtOffset(*(end() - 2), *(end() - 1), (*(end() - 2)).distanceTo(*(end() - 1)), lateralOffset);
269  }
270 }
271 
272 
273 Position
274 PositionVector::positionAtOffset2D(double pos, double lateralOffset) const {
275  if (size() == 0) {
276  return Position::INVALID;
277  }
278  if (size() == 1) {
279  return front();
280  }
281  const_iterator i = begin();
282  double seenLength = 0;
283  do {
284  const double nextLength = (*i).distanceTo2D(*(i + 1));
285  if (seenLength + nextLength > pos) {
286  return positionAtOffset2D(*i, *(i + 1), pos - seenLength, lateralOffset);
287  }
288  seenLength += nextLength;
289  } while (++i != end() - 1);
290  return back();
291 }
292 
293 
294 double
296  if ((size() == 0) || (size() == 1)) {
297  return INVALID_DOUBLE;
298  }
299  if (pos < 0) {
300  pos += length();
301  }
302  const_iterator i = begin();
303  double seenLength = 0;
304  do {
305  const Position& p1 = *i;
306  const Position& p2 = *(i + 1);
307  const double nextLength = p1.distanceTo(p2);
308  if (seenLength + nextLength > pos) {
309  return p1.angleTo2D(p2);
310  }
311  seenLength += nextLength;
312  } while (++i != end() - 1);
313  const Position& p1 = (*this)[-2];
314  const Position& p2 = back();
315  return p1.angleTo2D(p2);
316 }
317 
318 
319 double
322 }
323 
324 
325 double
327  if (size() == 0) {
328  return INVALID_DOUBLE;
329  }
330  const_iterator i = begin();
331  double seenLength = 0;
332  do {
333  const Position& p1 = *i;
334  const Position& p2 = *(i + 1);
335  const double nextLength = p1.distanceTo(p2);
336  if (seenLength + nextLength > pos) {
337  return RAD2DEG(atan2(p2.z() - p1.z(), p1.distanceTo2D(p2)));
338  }
339  seenLength += nextLength;
340  } while (++i != end() - 1);
341  const Position& p1 = (*this)[-2];
342  const Position& p2 = back();
343  return RAD2DEG(atan2(p2.z() - p1.z(), p1.distanceTo2D(p2)));
344 }
345 
346 
347 Position
348 PositionVector::positionAtOffset(const Position& p1, const Position& p2, double pos, double lateralOffset) {
349  const double dist = p1.distanceTo(p2);
350  if (pos < 0. || dist < pos) {
351  return Position::INVALID;
352  }
353  if (lateralOffset != 0) {
354  if (dist == 0.) {
355  return Position::INVALID;
356  }
357  const Position offset = sideOffset(p1, p2, -lateralOffset); // move in the same direction as Position::move2side
358  if (pos == 0.) {
359  return p1 + offset;
360  }
361  return p1 + (p2 - p1) * (pos / dist) + offset;
362  }
363  if (pos == 0.) {
364  return p1;
365  }
366  return p1 + (p2 - p1) * (pos / dist);
367 }
368 
369 
370 Position
371 PositionVector::positionAtOffset2D(const Position& p1, const Position& p2, double pos, double lateralOffset) {
372  const double dist = p1.distanceTo2D(p2);
373  if (pos < 0 || dist < pos) {
374  return Position::INVALID;
375  }
376  if (lateralOffset != 0) {
377  const Position offset = sideOffset(p1, p2, -lateralOffset); // move in the same direction as Position::move2side
378  if (pos == 0.) {
379  return p1 + offset;
380  }
381  return p1 + (p2 - p1) * (pos / dist) + offset;
382  }
383  if (pos == 0.) {
384  return p1;
385  }
386  return p1 + (p2 - p1) * (pos / dist);
387 }
388 
389 
390 Boundary
392  Boundary ret;
393  for (const Position& i : *this) {
394  ret.add(i);
395  }
396  return ret;
397 }
398 
399 
400 Position
402  double x = 0;
403  double y = 0;
404  double z = 0;
405  for (const Position& i : *this) {
406  x += i.x();
407  y += i.y();
408  z += i.z();
409  }
410  return Position(x / (double) size(), y / (double) size(), z / (double)size());
411 }
412 
413 
414 Position
416  if (size() == 0) {
417  return Position::INVALID;
418  } else if (size() == 1) {
419  return (*this)[0];
420  } else if (size() == 2) {
421  return ((*this)[0] + (*this)[1]) * 0.5;
422  }
423  PositionVector tmp = *this;
424  if (!isClosed()) { // make sure its closed
425  tmp.push_back(tmp[0]);
426  }
427  // shift to origin to increase numerical stability
428  Position offset = tmp[0];
429  Position result;
430  tmp.sub(offset);
431  const int endIndex = (int)tmp.size() - 1;
432  double div = 0; // 6 * area including sign
433  double x = 0;
434  double y = 0;
435  if (tmp.area() != 0) { // numerical instability ?
436  // http://en.wikipedia.org/wiki/Polygon
437  for (int i = 0; i < endIndex; i++) {
438  const double z = tmp[i].x() * tmp[i + 1].y() - tmp[i + 1].x() * tmp[i].y();
439  div += z; // area formula
440  x += (tmp[i].x() + tmp[i + 1].x()) * z;
441  y += (tmp[i].y() + tmp[i + 1].y()) * z;
442  }
443  div *= 3; // 6 / 2, the 2 comes from the area formula
444  result = Position(x / div, y / div);
445  } else {
446  // compute by decomposing into line segments
447  // http://en.wikipedia.org/wiki/Centroid#By_geometric_decomposition
448  double lengthSum = 0;
449  for (int i = 0; i < endIndex; i++) {
450  double length = tmp[i].distanceTo(tmp[i + 1]);
451  x += (tmp[i].x() + tmp[i + 1].x()) * length / 2;
452  y += (tmp[i].y() + tmp[i + 1].y()) * length / 2;
453  lengthSum += length;
454  }
455  if (lengthSum == 0) {
456  // it is probably only one point
457  result = tmp[0];
458  }
459  result = Position(x / lengthSum, y / lengthSum) + offset;
460  }
461  return result + offset;
462 }
463 
464 
465 void
467  Position centroid = getCentroid();
468  for (int i = 0; i < static_cast<int>(size()); i++) {
469  (*this)[i] = centroid + (((*this)[i] - centroid) * factor);
470  }
471 }
472 
473 
474 void
476  Position centroid = getCentroid();
477  for (int i = 0; i < static_cast<int>(size()); i++) {
478  (*this)[i] = centroid + (((*this)[i] - centroid) + offset);
479  }
480 }
481 
482 
483 Position
485  if (size() == 1) {
486  return (*this)[0];
487  } else {
488  return positionAtOffset(double((length() / 2.)));
489  }
490 }
491 
492 
493 double
495  if (size() == 0) {
496  return 0;
497  }
498  double len = 0;
499  for (const_iterator i = begin(); i != end() - 1; i++) {
500  len += (*i).distanceTo(*(i + 1));
501  }
502  return len;
503 }
504 
505 
506 double
508  if (size() == 0) {
509  return 0;
510  }
511  double len = 0;
512  for (const_iterator i = begin(); i != end() - 1; i++) {
513  len += (*i).distanceTo2D(*(i + 1));
514  }
515  return len;
516 }
517 
518 
519 double
521  if (size() < 3) {
522  return 0;
523  }
524  double area = 0;
525  PositionVector tmp = *this;
526  if (!isClosed()) { // make sure its closed
527  tmp.push_back(tmp[0]);
528  }
529  const int endIndex = (int)tmp.size() - 1;
530  // http://en.wikipedia.org/wiki/Polygon
531  for (int i = 0; i < endIndex; i++) {
532  area += tmp[i].x() * tmp[i + 1].y() - tmp[i + 1].x() * tmp[i].y();
533  }
534  if (area < 0) { // we whether we had cw or ccw order
535  area *= -1;
536  }
537  return area / 2;
538 }
539 
540 
541 bool
542 PositionVector::partialWithin(const AbstractPoly& poly, double offset) const {
543  if (size() < 2) {
544  return false;
545  }
546  for (const_iterator i = begin(); i != end(); i++) {
547  if (poly.around(*i, offset)) {
548  return true;
549  }
550  }
551  return false;
552 }
553 
554 
555 bool
556 PositionVector::crosses(const Position& p1, const Position& p2) const {
557  return intersects(p1, p2);
558 }
559 
560 
561 std::pair<PositionVector, PositionVector>
562 PositionVector::splitAt(double where, bool use2D) const {
563  const double len = use2D ? length2D() : length();
564  if (size() < 2) {
565  throw InvalidArgument("Vector to short for splitting");
566  }
567  if (where < 0 || where > len) {
568  throw InvalidArgument("Invalid split position " + toString(where) + " for vector of length " + toString(len));
569  }
570  if (where <= POSITION_EPS || where >= len - POSITION_EPS) {
571  WRITE_WARNINGF("Splitting vector close to end (pos: %, length: %)", toString(where), toString(len));
572  }
573  PositionVector first, second;
574  first.push_back((*this)[0]);
575  double seen = 0;
576  const_iterator it = begin() + 1;
577  double next = use2D ? first.back().distanceTo2D(*it) : first.back().distanceTo(*it);
578  // see how many points we can add to first
579  while (where >= seen + next + POSITION_EPS) {
580  seen += next;
581  first.push_back(*it);
582  it++;
583  next = use2D ? first.back().distanceTo2D(*it) : first.back().distanceTo(*it);
584  }
585  if (fabs(where - (seen + next)) > POSITION_EPS || it == end() - 1) {
586  // we need to insert a new point because 'where' is not close to an
587  // existing point or it is to close to the endpoint
588  const Position p = (use2D
589  ? positionAtOffset2D(first.back(), *it, where - seen)
590  : positionAtOffset(first.back(), *it, where - seen));
591  first.push_back(p);
592  second.push_back(p);
593  } else {
594  first.push_back(*it);
595  }
596  // add the remaining points to second
597  for (; it != end(); it++) {
598  second.push_back(*it);
599  }
600  assert(first.size() >= 2);
601  assert(second.size() >= 2);
602  assert(first.back() == second.front());
603  assert(fabs((use2D ? first.length2D() + second.length2D() : first.length() + second.length()) - len) < 2 * POSITION_EPS);
604  return std::pair<PositionVector, PositionVector>(first, second);
605 }
606 
607 
608 std::ostream&
609 operator<<(std::ostream& os, const PositionVector& geom) {
610  for (PositionVector::const_iterator i = geom.begin(); i != geom.end(); i++) {
611  if (i != geom.begin()) {
612  os << " ";
613  }
614  os << (*i);
615  }
616  return os;
617 }
618 
619 
620 void
622  std::sort(begin(), end(), as_poly_cw_sorter());
623 }
624 
625 
626 void
627 PositionVector::add(double xoff, double yoff, double zoff) {
628  for (int i = 0; i < (int)size(); i++) {
629  (*this)[i].add(xoff, yoff, zoff);
630  }
631 }
632 
633 
634 void
636  add(-offset.x(), -offset.y(), -offset.z());
637 }
638 
639 
640 void
642  add(offset.x(), offset.y(), offset.z());
643 }
644 
645 
647 PositionVector::added(const Position& offset) const {
648  PositionVector pv;
649  for (auto i1 = begin(); i1 != end(); ++i1) {
650  pv.push_back(*i1 + offset);
651  }
652  return pv;
653 }
654 
655 
656 void
658  for (int i = 0; i < (int)size(); i++) {
659  (*this)[i].mul(1, -1);
660  }
661 }
662 
663 
665 
666 
667 int
669  return atan2(p1.x(), p1.y()) < atan2(p2.x(), p2.y());
670 }
671 
672 
673 void
675  std::sort(begin(), end(), increasing_x_y_sorter());
676 }
677 
678 
680 
681 
682 int
684  if (p1.x() != p2.x()) {
685  return p1.x() < p2.x();
686  }
687  return p1.y() < p2.y();
688 }
689 
690 
691 double
692 PositionVector::isLeft(const Position& P0, const Position& P1, const Position& P2) const {
693  return (P1.x() - P0.x()) * (P2.y() - P0.y()) - (P2.x() - P0.x()) * (P1.y() - P0.y());
694 }
695 
696 
697 void
698 PositionVector::append(const PositionVector& v, double sameThreshold) {
699  if ((size() > 0) && (v.size() > 0) && (back().distanceTo(v[0]) < sameThreshold)) {
700  copy(v.begin() + 1, v.end(), back_inserter(*this));
701  } else {
702  copy(v.begin(), v.end(), back_inserter(*this));
703  }
704 }
705 
706 
707 void
708 PositionVector::prepend(const PositionVector& v, double sameThreshold) {
709  if ((size() > 0) && (v.size() > 0) && (front().distanceTo(v.back()) < sameThreshold)) {
710  insert(begin(), v.begin(), v.end() - 1);
711  } else {
712  insert(begin(), v.begin(), v.end());
713  }
714 }
715 
716 
718 PositionVector::getSubpart(double beginOffset, double endOffset) const {
719  PositionVector ret;
720  Position begPos = front();
721  if (beginOffset > POSITION_EPS) {
722  begPos = positionAtOffset(beginOffset);
723  }
724  Position endPos = back();
725  if (endOffset < length() - POSITION_EPS) {
726  endPos = positionAtOffset(endOffset);
727  }
728  ret.push_back(begPos);
729 
730  double seen = 0;
731  const_iterator i = begin();
732  // skip previous segments
733  while ((i + 1) != end()
734  &&
735  seen + (*i).distanceTo(*(i + 1)) < beginOffset) {
736  seen += (*i).distanceTo(*(i + 1));
737  i++;
738  }
739  // append segments in between
740  while ((i + 1) != end()
741  &&
742  seen + (*i).distanceTo(*(i + 1)) < endOffset) {
743 
744  ret.push_back_noDoublePos(*(i + 1));
745  seen += (*i).distanceTo(*(i + 1));
746  i++;
747  }
748  // append end
749  ret.push_back_noDoublePos(endPos);
750  if (ret.size() == 1) {
751  ret.push_back(endPos);
752  }
753  return ret;
754 }
755 
756 
758 PositionVector::getSubpart2D(double beginOffset, double endOffset) const {
759  if (size() == 0) {
760  return PositionVector();
761  }
762  PositionVector ret;
763  Position begPos = front();
764  if (beginOffset > POSITION_EPS) {
765  begPos = positionAtOffset2D(beginOffset);
766  }
767  Position endPos = back();
768  if (endOffset < length2D() - POSITION_EPS) {
769  endPos = positionAtOffset2D(endOffset);
770  }
771  ret.push_back(begPos);
772 
773  double seen = 0;
774  const_iterator i = begin();
775  // skip previous segments
776  while ((i + 1) != end()
777  &&
778  seen + (*i).distanceTo2D(*(i + 1)) < beginOffset) {
779  seen += (*i).distanceTo2D(*(i + 1));
780  i++;
781  }
782  // append segments in between
783  while ((i + 1) != end()
784  &&
785  seen + (*i).distanceTo2D(*(i + 1)) < endOffset) {
786 
787  ret.push_back_noDoublePos(*(i + 1));
788  seen += (*i).distanceTo2D(*(i + 1));
789  i++;
790  }
791  // append end
792  ret.push_back_noDoublePos(endPos);
793  if (ret.size() == 1) {
794  ret.push_back(endPos);
795  }
796  return ret;
797 }
798 
799 
801 PositionVector::getSubpartByIndex(int beginIndex, int count) const {
802  if (size() == 0) {
803  return PositionVector();
804  }
805  if (beginIndex < 0) {
806  beginIndex += (int)size();
807  }
808  assert(count >= 0);
809  assert(beginIndex < (int)size());
810  assert(beginIndex + count <= (int)size());
811  PositionVector result;
812  for (int i = beginIndex; i < beginIndex + count; ++i) {
813  result.push_back((*this)[i]);
814  }
815  return result;
816 }
817 
818 
819 double
821  if (size() == 0) {
822  return INVALID_DOUBLE;
823  }
824  return front().angleTo2D(back());
825 }
826 
827 
828 double
829 PositionVector::nearest_offset_to_point2D(const Position& p, bool perpendicular) const {
830  if (size() == 0) {
831  return INVALID_DOUBLE;
832  }
833  double minDist = std::numeric_limits<double>::max();
834  double nearestPos = GeomHelper::INVALID_OFFSET;
835  double seen = 0;
836  for (const_iterator i = begin(); i != end() - 1; i++) {
837  const double pos =
838  GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, perpendicular);
839  const double dist = pos == GeomHelper::INVALID_OFFSET ? minDist : p.distanceTo2D(positionAtOffset2D(*i, *(i + 1), pos));
840  if (dist < minDist) {
841  nearestPos = pos + seen;
842  minDist = dist;
843  }
844  if (perpendicular && i != begin() && pos == GeomHelper::INVALID_OFFSET) {
845  // even if perpendicular is set we still need to check the distance to the inner points
846  const double cornerDist = p.distanceTo2D(*i);
847  if (cornerDist < minDist) {
848  const double pos1 =
849  GeomHelper::nearest_offset_on_line_to_point2D(*(i - 1), *i, p, false);
850  const double pos2 =
851  GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, false);
852  if (pos1 == (*(i - 1)).distanceTo2D(*i) && pos2 == 0.) {
853  nearestPos = seen;
854  minDist = cornerDist;
855  }
856  }
857  }
858  seen += (*i).distanceTo2D(*(i + 1));
859  }
860  return nearestPos;
861 }
862 
863 
864 double
865 PositionVector::nearest_offset_to_point25D(const Position& p, bool perpendicular) const {
866  if (size() == 0) {
867  return INVALID_DOUBLE;
868  }
869  double minDist = std::numeric_limits<double>::max();
870  double nearestPos = GeomHelper::INVALID_OFFSET;
871  double seen = 0;
872  for (const_iterator i = begin(); i != end() - 1; i++) {
873  const double pos =
874  GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, perpendicular);
875  const double dist = pos == GeomHelper::INVALID_OFFSET ? minDist : p.distanceTo2D(positionAtOffset2D(*i, *(i + 1), pos));
876  if (dist < minDist) {
877  const double pos25D = pos * (*i).distanceTo(*(i + 1)) / (*i).distanceTo2D(*(i + 1));
878  nearestPos = pos25D + seen;
879  minDist = dist;
880  }
881  if (perpendicular && i != begin() && pos == GeomHelper::INVALID_OFFSET) {
882  // even if perpendicular is set we still need to check the distance to the inner points
883  const double cornerDist = p.distanceTo2D(*i);
884  if (cornerDist < minDist) {
885  const double pos1 =
886  GeomHelper::nearest_offset_on_line_to_point2D(*(i - 1), *i, p, false);
887  const double pos2 =
888  GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, false);
889  if (pos1 == (*(i - 1)).distanceTo2D(*i) && pos2 == 0.) {
890  nearestPos = seen;
891  minDist = cornerDist;
892  }
893  }
894  }
895  seen += (*i).distanceTo(*(i + 1));
896  }
897  return nearestPos;
898 }
899 
900 
901 Position
903  if (size() == 0) {
904  return Position::INVALID;
905  }
906  // @toDo this duplicates most of the code in nearest_offset_to_point2D. It should be refactored
907  if (extend) {
908  PositionVector extended = *this;
909  const double dist = 2 * distance2D(p);
910  extended.extrapolate(dist);
911  return extended.transformToVectorCoordinates(p) - Position(dist, 0);
912  }
913  double minDist = std::numeric_limits<double>::max();
914  double nearestPos = -1;
915  double seen = 0;
916  int sign = 1;
917  for (const_iterator i = begin(); i != end() - 1; i++) {
918  const double pos =
919  GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, true);
920  const double dist = pos < 0 ? minDist : p.distanceTo2D(positionAtOffset(*i, *(i + 1), pos));
921  if (dist < minDist) {
922  nearestPos = pos + seen;
923  minDist = dist;
924  sign = isLeft(*i, *(i + 1), p) >= 0 ? -1 : 1;
925  }
926  if (i != begin() && pos == GeomHelper::INVALID_OFFSET) {
927  // even if perpendicular is set we still need to check the distance to the inner points
928  const double cornerDist = p.distanceTo2D(*i);
929  if (cornerDist < minDist) {
930  const double pos1 =
931  GeomHelper::nearest_offset_on_line_to_point2D(*(i - 1), *i, p, false);
932  const double pos2 =
933  GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, false);
934  if (pos1 == (*(i - 1)).distanceTo2D(*i) && pos2 == 0.) {
935  nearestPos = seen;
936  minDist = cornerDist;
937  sign = isLeft(*(i - 1), *i, p) >= 0 ? -1 : 1;
938  }
939  }
940  }
941  seen += (*i).distanceTo2D(*(i + 1));
942  }
943  if (nearestPos != -1) {
944  return Position(nearestPos, sign * minDist);
945  } else {
946  return Position::INVALID;
947  }
948 }
949 
950 
951 int
952 PositionVector::indexOfClosest(const Position& p, bool twoD) const {
953  if (size() == 0) {
954  return -1;
955  }
956  double minDist = std::numeric_limits<double>::max();
957  double dist;
958  int closest = 0;
959  for (int i = 0; i < (int)size(); i++) {
960  const Position& p2 = (*this)[i];
961  dist = twoD ? p.distanceTo2D(p2) : p.distanceTo(p2);
962  if (dist < minDist) {
963  closest = i;
964  minDist = dist;
965  }
966  }
967  return closest;
968 }
969 
970 
971 int
973  if (size() == 0) {
974  return -1;
975  }
976  double minDist = std::numeric_limits<double>::max();
977  int insertionIndex = 1;
978  for (int i = 0; i < (int)size() - 1; i++) {
979  const double length = GeomHelper::nearest_offset_on_line_to_point2D((*this)[i], (*this)[i + 1], p, false);
980  const Position& outIntersection = PositionVector::positionAtOffset2D((*this)[i], (*this)[i + 1], length);
981  const double dist = p.distanceTo2D(outIntersection);
982  if (dist < minDist) {
983  insertionIndex = i + 1;
984  minDist = dist;
985  }
986  }
987  // check if we have to adjust Position Z
988  if (interpolateZ) {
989  // obtain previous and next Z
990  const double previousZ = (begin() + (insertionIndex - 1))->z();
991  const double nextZ = (begin() + insertionIndex)->z();
992  // insert new position using x and y of p, and the new z
993  insert(begin() + insertionIndex, Position(p.x(), p.y(), ((previousZ + nextZ) / 2.0)));
994  } else {
995  insert(begin() + insertionIndex, p);
996  }
997  return insertionIndex;
998 }
999 
1000 
1001 int
1003  if (size() == 0) {
1004  return -1;
1005  }
1006  double minDist = std::numeric_limits<double>::max();
1007  int removalIndex = 0;
1008  for (int i = 0; i < (int)size(); i++) {
1009  const double dist = p.distanceTo2D((*this)[i]);
1010  if (dist < minDist) {
1011  removalIndex = i;
1012  minDist = dist;
1013  }
1014  }
1015  erase(begin() + removalIndex);
1016  return removalIndex;
1017 }
1018 
1019 
1020 std::vector<double>
1022  std::vector<double> ret;
1023  if (other.size() == 0) {
1024  return ret;
1025  }
1026  for (const_iterator i = other.begin(); i != other.end() - 1; i++) {
1027  std::vector<double> atSegment = intersectsAtLengths2D(*i, *(i + 1));
1028  copy(atSegment.begin(), atSegment.end(), back_inserter(ret));
1029  }
1030  return ret;
1031 }
1032 
1033 
1034 std::vector<double>
1036  std::vector<double> ret;
1037  if (size() == 0) {
1038  return ret;
1039  }
1040  double pos = 0;
1041  for (const_iterator i = begin(); i != end() - 1; i++) {
1042  const Position& p1 = *i;
1043  const Position& p2 = *(i + 1);
1044  double x, y, m;
1045  if (intersects(p1, p2, lp1, lp2, 0., &x, &y, &m)) {
1046  ret.push_back(Position(x, y).distanceTo2D(p1) + pos);
1047  }
1048  pos += p1.distanceTo2D(p2);
1049  }
1050  return ret;
1051 }
1052 
1053 
1054 void
1055 PositionVector::extrapolate(const double val, const bool onlyFirst, const bool onlyLast) {
1056  if (size() > 0) {
1057  Position& p1 = (*this)[0];
1058  Position& p2 = (*this)[1];
1059  const Position offset = (p2 - p1) * (val / p1.distanceTo(p2));
1060  if (!onlyLast) {
1061  p1.sub(offset);
1062  }
1063  if (!onlyFirst) {
1064  if (size() == 2) {
1065  p2.add(offset);
1066  } else {
1067  const Position& e1 = (*this)[-2];
1068  Position& e2 = (*this)[-1];
1069  e2.sub((e1 - e2) * (val / e1.distanceTo(e2)));
1070  }
1071  }
1072  }
1073 }
1074 
1075 
1076 void
1077 PositionVector::extrapolate2D(const double val, const bool onlyFirst) {
1078  if (size() > 0) {
1079  Position& p1 = (*this)[0];
1080  Position& p2 = (*this)[1];
1081  if (p1.distanceTo2D(p2) > 0) {
1082  const Position offset = (p2 - p1) * (val / p1.distanceTo2D(p2));
1083  p1.sub(offset);
1084  if (!onlyFirst) {
1085  if (size() == 2) {
1086  p2.add(offset);
1087  } else {
1088  const Position& e1 = (*this)[-2];
1089  Position& e2 = (*this)[-1];
1090  e2.sub((e1 - e2) * (val / e1.distanceTo2D(e2)));
1091  }
1092  }
1093  }
1094  }
1095 }
1096 
1097 
1100  PositionVector ret;
1101  for (const_reverse_iterator i = rbegin(); i != rend(); i++) {
1102  ret.push_back(*i);
1103  }
1104  return ret;
1105 }
1106 
1107 
1108 Position
1109 PositionVector::sideOffset(const Position& beg, const Position& end, const double amount) {
1110  const double scale = amount / beg.distanceTo2D(end);
1111  return Position((beg.y() - end.y()) * scale, (end.x() - beg.x()) * scale);
1112 }
1113 
1114 
1115 void
1116 PositionVector::move2side(double amount, double maxExtension) {
1117  if (size() < 2) {
1118  return;
1119  }
1120  removeDoublePoints(POSITION_EPS, true);
1121  if (length2D() == 0 || amount == 0) {
1122  return;
1123  }
1124  PositionVector shape;
1125  std::vector<int> recheck;
1126  for (int i = 0; i < static_cast<int>(size()); i++) {
1127  if (i == 0) {
1128  const Position& from = (*this)[i];
1129  const Position& to = (*this)[i + 1];
1130  if (from != to) {
1131  shape.push_back(from - sideOffset(from, to, amount));
1132 #ifdef DEBUG_MOVE2SIDE
1133  if (gDebugFlag1) std::cout << " " << i << "a=" << shape.back() << "\n";
1134 #endif
1135  }
1136  } else if (i == static_cast<int>(size()) - 1) {
1137  const Position& from = (*this)[i - 1];
1138  const Position& to = (*this)[i];
1139  if (from != to) {
1140  shape.push_back(to - sideOffset(from, to, amount));
1141 #ifdef DEBUG_MOVE2SIDE
1142  if (gDebugFlag1) std::cout << " " << i << "b=" << shape.back() << "\n";
1143 #endif
1144  }
1145  } else {
1146  const Position& from = (*this)[i - 1];
1147  const Position& me = (*this)[i];
1148  const Position& to = (*this)[i + 1];
1149  PositionVector fromMe(from, me);
1150  fromMe.extrapolate2D(me.distanceTo2D(to));
1151  const double extrapolateDev = fromMe[1].distanceTo2D(to);
1152  if (fabs(extrapolateDev) < POSITION_EPS) {
1153  // parallel case, just shift the middle point
1154  shape.push_back(me - sideOffset(from, to, amount));
1155 #ifdef DEBUG_MOVE2SIDE
1156  if (gDebugFlag1) std::cout << " " << i << "c=" << shape.back() << "\n";
1157 #endif
1158  } else if (fabs(extrapolateDev - 2 * me.distanceTo2D(to)) < POSITION_EPS) {
1159  // counterparallel case, just shift the middle point
1160  PositionVector fromMe2(from, me);
1161  fromMe2.extrapolate2D(amount);
1162  shape.push_back(fromMe2[1]);
1163 #ifdef DEBUG_MOVE2SIDE
1164  if (gDebugFlag1) std::cout << " " << i << "d=" << shape.back() << " " << i << "_from=" << from << " " << i << "_me=" << me << " " << i << "_to=" << to << "\n";
1165 #endif
1166  } else {
1167  Position offsets = sideOffset(from, me, amount);
1168  Position offsets2 = sideOffset(me, to, amount);
1169  PositionVector l1(from - offsets, me - offsets);
1170  PositionVector l2(me - offsets2, to - offsets2);
1171  Position meNew = l1.intersectionPosition2D(l2[0], l2[1], maxExtension);
1172  if (meNew == Position::INVALID) {
1173  recheck.push_back(i);
1174  continue;
1175  }
1176  meNew = meNew + Position(0, 0, me.z());
1177  shape.push_back(meNew);
1178 #ifdef DEBUG_MOVE2SIDE
1179  if (gDebugFlag1) std::cout << " " << i << "e=" << shape.back() << "\n";
1180 #endif
1181  }
1182  // copy original z value
1183  shape.back().set(shape.back().x(), shape.back().y(), me.z());
1184  const double angle = localAngle(from, me, to);
1185  if (fabs(angle) > NUMERICAL_EPS) {
1186  const double length = (i == 1 || i + 2 == (int)size()
1187  ? MIN2(from.distanceTo2D(me), me.distanceTo2D(to)) * 2
1188  : (from.distanceTo2D(me) + me.distanceTo2D(to)));
1189  const double radius = length / angle;
1190 #ifdef DEBUG_MOVE2SIDE
1191  if (gDebugFlag1) std::cout << " i=" << i << " a=" << RAD2DEG(angle) << " l=" << length << " r=" << radius << " t=" << amount * 1.8 << "\n";
1192 #endif
1193  if ((radius < 0 && -radius < amount * 1.8) || fabs(RAD2DEG(angle)) > 170) {
1194  recheck.push_back(i);
1195  }
1196  }
1197  }
1198  }
1199  if (!recheck.empty()) {
1200  // try to adjust positions to avoid clipping
1201  shape = *this;
1202  for (int i = (int)recheck.size() - 1; i >= 0; i--) {
1203  shape.erase(shape.begin() + recheck[i]);
1204  }
1205  shape.move2side(amount, maxExtension);
1206  }
1207  *this = shape;
1208 }
1209 
1210 
1211 void
1212 PositionVector::move2sideCustom(std::vector<double> amount, double maxExtension) {
1213  if (size() < 2) {
1214  return;
1215  }
1216  if (length2D() == 0) {
1217  return;
1218  }
1219  if (size() != amount.size()) {
1220  throw InvalidArgument("Numer of offsets (" + toString(amount.size())
1221  + ") does not match number of points (" + toString(size()) + ")");
1222  }
1223  PositionVector shape;
1224  for (int i = 0; i < static_cast<int>(size()); i++) {
1225  if (i == 0) {
1226  const Position& from = (*this)[i];
1227  const Position& to = (*this)[i + 1];
1228  if (from != to) {
1229  shape.push_back(from - sideOffset(from, to, amount[i]));
1230  }
1231  } else if (i == static_cast<int>(size()) - 1) {
1232  const Position& from = (*this)[i - 1];
1233  const Position& to = (*this)[i];
1234  if (from != to) {
1235  shape.push_back(to - sideOffset(from, to, amount[i]));
1236  }
1237  } else {
1238  const Position& from = (*this)[i - 1];
1239  const Position& me = (*this)[i];
1240  const Position& to = (*this)[i + 1];
1241  PositionVector fromMe(from, me);
1242  fromMe.extrapolate2D(me.distanceTo2D(to));
1243  const double extrapolateDev = fromMe[1].distanceTo2D(to);
1244  if (fabs(extrapolateDev) < POSITION_EPS) {
1245  // parallel case, just shift the middle point
1246  shape.push_back(me - sideOffset(from, to, amount[i]));
1247  } else if (fabs(extrapolateDev - 2 * me.distanceTo2D(to)) < POSITION_EPS) {
1248  // counterparallel case, just shift the middle point
1249  PositionVector fromMe2(from, me);
1250  fromMe2.extrapolate2D(amount[i]);
1251  shape.push_back(fromMe2[1]);
1252  } else {
1253  Position offsets = sideOffset(from, me, amount[i]);
1254  Position offsets2 = sideOffset(me, to, amount[i]);
1255  PositionVector l1(from - offsets, me - offsets);
1256  PositionVector l2(me - offsets2, to - offsets2);
1257  Position meNew = l1.intersectionPosition2D(l2[0], l2[1], maxExtension);
1258  if (meNew == Position::INVALID) {
1259  continue;
1260  }
1261  meNew = meNew + Position(0, 0, me.z());
1262  shape.push_back(meNew);
1263  }
1264  // copy original z value
1265  shape.back().set(shape.back().x(), shape.back().y(), me.z());
1266  }
1267  }
1268  *this = shape;
1269 }
1270 
1271 double
1272 PositionVector::localAngle(const Position& from, const Position& pos, const Position& to) {
1273  return GeomHelper::angleDiff(from.angleTo2D(pos), pos.angleTo2D(to));
1274 }
1275 
1276 double
1278  if ((pos + 1) < (int)size()) {
1279  return (*this)[pos].angleTo2D((*this)[pos + 1]);
1280  } else {
1281  return INVALID_DOUBLE;
1282  }
1283 }
1284 
1285 
1286 void
1288  if ((size() != 0) && ((*this)[0] != back())) {
1289  push_back((*this)[0]);
1290  }
1291 }
1292 
1293 
1294 std::vector<double>
1295 PositionVector::distances(const PositionVector& s, bool perpendicular) const {
1296  std::vector<double> ret;
1297  const_iterator i;
1298  for (i = begin(); i != end(); i++) {
1299  const double dist = s.distance2D(*i, perpendicular);
1300  if (dist != GeomHelper::INVALID_OFFSET) {
1301  ret.push_back(dist);
1302  }
1303  }
1304  for (i = s.begin(); i != s.end(); i++) {
1305  const double dist = distance2D(*i, perpendicular);
1306  if (dist != GeomHelper::INVALID_OFFSET) {
1307  ret.push_back(dist);
1308  }
1309  }
1310  return ret;
1311 }
1312 
1313 
1314 double
1315 PositionVector::distance2D(const Position& p, bool perpendicular) const {
1316  if (size() == 0) {
1317  return std::numeric_limits<double>::max();
1318  } else if (size() == 1) {
1319  return front().distanceTo(p);
1320  }
1321  const double nearestOffset = nearest_offset_to_point2D(p, perpendicular);
1322  if (nearestOffset == GeomHelper::INVALID_OFFSET) {
1324  } else {
1325  return p.distanceTo2D(positionAtOffset2D(nearestOffset));
1326  }
1327 }
1328 
1329 
1330 void
1332  if (empty()) {
1333  push_back(p);
1334  } else {
1335  insert(begin(), p);
1336  }
1337 }
1338 
1339 
1340 void
1342  if (empty()) {
1343  throw ProcessError("PositionVector is empty");
1344  } else {
1345  erase(begin());
1346  }
1347 }
1348 
1349 
1350 void
1352  if (size() == 0 || !p.almostSame(back())) {
1353  push_back(p);
1354  }
1355 }
1356 
1357 
1358 void
1360  if ((size() == 0) || !p.almostSame(front())) {
1361  push_front(p);
1362  }
1363 }
1364 
1365 
1366 void
1367 PositionVector::insert_noDoublePos(const std::vector<Position>::iterator& at, const Position& p) {
1368  if (at == begin()) {
1370  } else if (at == end()) {
1372  } else {
1373  if (!p.almostSame(*at) && !p.almostSame(*(at - 1))) {
1374  insert(at, p);
1375  }
1376  }
1377 }
1378 
1379 
1380 bool
1382  return (size() >= 2) && ((*this)[0] == back());
1383 }
1384 
1385 
1386 bool
1388  // iterate over all positions and check if is NAN
1389  for (auto i = begin(); i != end(); i++) {
1390  if (i->isNAN()) {
1391  return true;
1392  }
1393  }
1394  // all ok, then return false
1395  return false;
1396 }
1397 
1398 
1399 void
1400 PositionVector::removeDoublePoints(double minDist, bool assertLength, int beginOffset, int endOffset, bool resample) {
1401  int curSize = (int)size() - beginOffset - endOffset;
1402  if (curSize > 1) {
1403  iterator last = begin() + beginOffset;
1404  for (iterator i = last + 1; i != (end() - endOffset) && (!assertLength || curSize > 2);) {
1405  if (last->almostSame(*i, minDist)) {
1406  if (i + 1 == end() - endOffset) {
1407  // special case: keep the last point and remove the next-to-last
1408  if (resample && last > begin() && (last - 1)->distanceTo(*i) >= 2 * minDist) {
1409  // resample rather than remove point after a long segment
1410  const double shiftBack = minDist - last->distanceTo(*i);
1411  //if (gDebugFlag1) std::cout << " resample endOffset beforeLast=" << *(last - 1) << " last=" << *last << " i=" << *i;
1412  (*last) = positionAtOffset(*(last - 1), *last, (last - 1)->distanceTo(*last) - shiftBack);
1413  //if (gDebugFlag1) std::cout << " lastNew=" << *last;
1414  last = i;
1415  ++i;
1416  } else {
1417  erase(last);
1418  i = end() - endOffset;
1419  }
1420  } else {
1421  if (resample && i + 1 != end() && last->distanceTo(*(i + 1)) >= 2 * minDist) {
1422  // resample rather than remove points before a long segment
1423  const double shiftForward = minDist - last->distanceTo(*i);
1424  //if (gDebugFlag1) std::cout << " resample last=" << *last << " i=" << *i << " next=" << *(i + 1);
1425  (*i) = positionAtOffset(*i, *(i + 1), shiftForward);
1426  //if (gDebugFlag1) std::cout << " iNew=" << *i << "\n";
1427  last = i;
1428  ++i;
1429  } else {
1430  i = erase(i);
1431  }
1432  }
1433  curSize--;
1434  } else {
1435  last = i;
1436  ++i;
1437  }
1438  }
1439  }
1440 }
1441 
1442 
1443 bool
1445  return static_cast<vp>(*this) == static_cast<vp>(v2);
1446 }
1447 
1448 
1449 bool
1451  return static_cast<vp>(*this) != static_cast<vp>(v2);
1452 }
1453 
1456  if (length() != v2.length()) {
1457  WRITE_ERROR("Trying to substract PositionVectors of different lengths.");
1458  }
1459  PositionVector pv;
1460  auto i1 = begin();
1461  auto i2 = v2.begin();
1462  while (i1 != end()) {
1463  pv.add(*i1 - *i2);
1464  }
1465  return pv;
1466 }
1467 
1470  if (length() != v2.length()) {
1471  WRITE_ERROR("Trying to substract PositionVectors of different lengths.");
1472  }
1473  PositionVector pv;
1474  auto i1 = begin();
1475  auto i2 = v2.begin();
1476  while (i1 != end()) {
1477  pv.add(*i1 + *i2);
1478  }
1479  return pv;
1480 }
1481 
1482 bool
1484  if (size() < 2) {
1485  return false;
1486  }
1487  for (const_iterator i = begin(); i != end() - 1; i++) {
1488  if ((*i).z() != (*(i + 1)).z()) {
1489  return true;
1490  }
1491  }
1492  return false;
1493 }
1494 
1495 
1496 bool
1497 PositionVector::intersects(const Position& p11, const Position& p12, const Position& p21, const Position& p22, const double withinDist, double* x, double* y, double* mu) {
1498  const double eps = std::numeric_limits<double>::epsilon();
1499  const double denominator = (p22.y() - p21.y()) * (p12.x() - p11.x()) - (p22.x() - p21.x()) * (p12.y() - p11.y());
1500  const double numera = (p22.x() - p21.x()) * (p11.y() - p21.y()) - (p22.y() - p21.y()) * (p11.x() - p21.x());
1501  const double numerb = (p12.x() - p11.x()) * (p11.y() - p21.y()) - (p12.y() - p11.y()) * (p11.x() - p21.x());
1502  /* Are the lines coincident? */
1503  if (fabs(numera) < eps && fabs(numerb) < eps && fabs(denominator) < eps) {
1504  double a1;
1505  double a2;
1506  double a3;
1507  double a4;
1508  double a = -1e12;
1509  if (p11.x() != p12.x()) {
1510  a1 = p11.x() < p12.x() ? p11.x() : p12.x();
1511  a2 = p11.x() < p12.x() ? p12.x() : p11.x();
1512  a3 = p21.x() < p22.x() ? p21.x() : p22.x();
1513  a4 = p21.x() < p22.x() ? p22.x() : p21.x();
1514  } else {
1515  a1 = p11.y() < p12.y() ? p11.y() : p12.y();
1516  a2 = p11.y() < p12.y() ? p12.y() : p11.y();
1517  a3 = p21.y() < p22.y() ? p21.y() : p22.y();
1518  a4 = p21.y() < p22.y() ? p22.y() : p21.y();
1519  }
1520  if (a1 <= a3 && a3 <= a2) {
1521  if (a4 < a2) {
1522  a = (a3 + a4) / 2;
1523  } else {
1524  a = (a2 + a3) / 2;
1525  }
1526  }
1527  if (a3 <= a1 && a1 <= a4) {
1528  if (a2 < a4) {
1529  a = (a1 + a2) / 2;
1530  } else {
1531  a = (a1 + a4) / 2;
1532  }
1533  }
1534  if (a != -1e12) {
1535  if (x != nullptr) {
1536  if (p11.x() != p12.x()) {
1537  *mu = (a - p11.x()) / (p12.x() - p11.x());
1538  *x = a;
1539  *y = p11.y() + (*mu) * (p12.y() - p11.y());
1540  } else {
1541  *x = p11.x();
1542  *y = a;
1543  if (p12.y() == p11.y()) {
1544  *mu = 0;
1545  } else {
1546  *mu = (a - p11.y()) / (p12.y() - p11.y());
1547  }
1548  }
1549  }
1550  return true;
1551  }
1552  return false;
1553  }
1554  /* Are the lines parallel */
1555  if (fabs(denominator) < eps) {
1556  return false;
1557  }
1558  /* Is the intersection along the segments */
1559  double mua = numera / denominator;
1560  /* reduce rounding errors for lines ending in the same point */
1561  if (fabs(p12.x() - p22.x()) < eps && fabs(p12.y() - p22.y()) < eps) {
1562  mua = 1.;
1563  } else {
1564  const double offseta = withinDist / p11.distanceTo2D(p12);
1565  const double offsetb = withinDist / p21.distanceTo2D(p22);
1566  const double mub = numerb / denominator;
1567  if (mua < -offseta || mua > 1 + offseta || mub < -offsetb || mub > 1 + offsetb) {
1568  return false;
1569  }
1570  }
1571  if (x != nullptr) {
1572  *x = p11.x() + mua * (p12.x() - p11.x());
1573  *y = p11.y() + mua * (p12.y() - p11.y());
1574  *mu = mua;
1575  }
1576  return true;
1577 }
1578 
1579 
1580 void
1582  const double s = sin(angle);
1583  const double c = cos(angle);
1584  for (int i = 0; i < (int)size(); i++) {
1585  const double x = (*this)[i].x();
1586  const double y = (*this)[i].y();
1587  const double z = (*this)[i].z();
1588  const double xnew = x * c - y * s;
1589  const double ynew = x * s + y * c;
1590  (*this)[i].set(xnew, ynew, z);
1591  }
1592 }
1593 
1594 
1597  PositionVector result = *this;
1598  bool changed = true;
1599  while (changed && result.size() > 3) {
1600  changed = false;
1601  for (int i = 0; i < (int)result.size(); i++) {
1602  const Position& p1 = result[i];
1603  const Position& p2 = result[(i + 2) % result.size()];
1604  const int middleIndex = (i + 1) % result.size();
1605  const Position& p0 = result[middleIndex];
1606  // https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Line_defined_by_two_points
1607  const double triangleArea2 = fabs((p2.y() - p1.y()) * p0.x() - (p2.x() - p1.x()) * p0.y() + p2.x() * p1.y() - p2.y() * p1.x());
1608  const double distIK = p1.distanceTo2D(p2);
1609  if (distIK > NUMERICAL_EPS && triangleArea2 / distIK < NUMERICAL_EPS) {
1610  changed = true;
1611  result.erase(result.begin() + middleIndex);
1612  break;
1613  }
1614  }
1615  }
1616  return result;
1617 }
1618 
1619 
1621 PositionVector::getOrthogonal(const Position& p, double extend, bool before, double length, double deg) const {
1622  PositionVector result;
1623  PositionVector tmp = *this;
1624  tmp.extrapolate2D(extend);
1625  const double baseOffset = tmp.nearest_offset_to_point2D(p);
1626  if (baseOffset == GeomHelper::INVALID_OFFSET || size() < 2) {
1627  // fail
1628  return result;
1629  }
1630  Position base = tmp.positionAtOffset2D(baseOffset);
1631  const int closestIndex = tmp.indexOfClosest(base);
1632  const double closestOffset = tmp.offsetAtIndex2D(closestIndex);
1633  result.push_back(base);
1634  if (fabs(baseOffset - closestOffset) > NUMERICAL_EPS) {
1635  result.push_back(tmp[closestIndex]);
1636  if ((closestOffset < baseOffset) != before) {
1637  deg *= -1;
1638  }
1639  } else if (before) {
1640  // take the segment before closestIndex if possible
1641  if (closestIndex > 0) {
1642  result.push_back(tmp[closestIndex - 1]);
1643  } else {
1644  result.push_back(tmp[1]);
1645  deg *= -1;
1646  }
1647  } else {
1648  // take the segment after closestIndex if possible
1649  if (closestIndex < (int)size() - 1) {
1650  result.push_back(tmp[closestIndex + 1]);
1651  } else {
1652  result.push_back(tmp[-1]);
1653  deg *= -1;
1654  }
1655  }
1656  result = result.getSubpart2D(0, length);
1657  // rotate around base
1658  result.add(base * -1);
1659  result.rotate2D(DEG2RAD(deg));
1660  result.add(base);
1661  return result;
1662 }
1663 
1664 
1667  PositionVector result = *this;
1668  if (size() == 0) {
1669  return result;
1670  }
1671  const double z0 = (*this)[0].z();
1672  // the z-delta of the first segment
1673  const double dz = (*this)[1].z() - z0;
1674  // if the shape only has 2 points it is as smooth as possible already
1675  if (size() > 2 && dz != 0) {
1676  dist = MIN2(dist, length2D());
1677  // check wether we need to insert a new point at dist
1678  Position pDist = positionAtOffset2D(dist);
1679  int iLast = indexOfClosest(pDist);
1680  // prevent close spacing to reduce impact of rounding errors in z-axis
1681  if (pDist.distanceTo2D((*this)[iLast]) > POSITION_EPS * 20) {
1682  iLast = result.insertAtClosest(pDist, false);
1683  }
1684  double dist2 = result.offsetAtIndex2D(iLast);
1685  const double dz2 = result[iLast].z() - z0;
1686  double seen = 0;
1687  for (int i = 1; i < iLast; ++i) {
1688  seen += result[i].distanceTo2D(result[i - 1]);
1689  result[i].set(result[i].x(), result[i].y(), z0 + dz2 * seen / dist2);
1690  }
1691  }
1692  return result;
1693 
1694 }
1695 
1696 
1698 PositionVector::interpolateZ(double zStart, double zEnd) const {
1699  PositionVector result = *this;
1700  if (size() == 0) {
1701  return result;
1702  }
1703  result[0].setz(zStart);
1704  result[-1].setz(zEnd);
1705  const double dz = zEnd - zStart;
1706  const double length = length2D();
1707  double seen = 0;
1708  for (int i = 1; i < (int)size() - 1; ++i) {
1709  seen += result[i].distanceTo2D(result[i - 1]);
1710  result[i].setz(zStart + dz * seen / length);
1711  }
1712  return result;
1713 }
1714 
1715 
1717 PositionVector::resample(double maxLength, const bool adjustEnd) const {
1718  PositionVector result;
1719  if (maxLength == 0) {
1720  return result;
1721  }
1722  const double length = length2D();
1723  if (length < POSITION_EPS) {
1724  return result;
1725  }
1726  maxLength = length / ceil(length / maxLength);
1727  for (double pos = 0; pos <= length; pos += maxLength) {
1728  result.push_back(positionAtOffset2D(pos));
1729  }
1730  // check if we have to adjust last element
1731  if (adjustEnd && !result.empty() && (result.back() != back())) {
1732  // add last element
1733  result.push_back(back());
1734  }
1735  return result;
1736 }
1737 
1738 
1739 double
1741  if (index < 0 || index >= (int)size()) {
1743  }
1744  double seen = 0;
1745  for (int i = 1; i <= index; ++i) {
1746  seen += (*this)[i].distanceTo2D((*this)[i - 1]);
1747  }
1748  return seen;
1749 }
1750 
1751 
1752 double
1753 PositionVector::getMaxGrade(double& maxJump) const {
1754  double result = 0;
1755  for (int i = 1; i < (int)size(); ++i) {
1756  const Position& p1 = (*this)[i - 1];
1757  const Position& p2 = (*this)[i];
1758  const double distZ = fabs(p1.z() - p2.z());
1759  const double dist2D = p1.distanceTo2D(p2);
1760  if (dist2D == 0) {
1761  maxJump = MAX2(maxJump, distZ);
1762  } else {
1763  result = MAX2(result, distZ / dist2D);
1764  }
1765  }
1766  return result;
1767 }
1768 
1769 
1771 PositionVector::bezier(int numPoints) {
1772  // inspired by David F. Rogers
1773  assert(size() < 33);
1774  static const double fac[33] = {
1775  1.0, 1.0, 2.0, 6.0, 24.0, 120.0, 720.0, 5040.0, 40320.0, 362880.0, 3628800.0, 39916800.0, 479001600.0,
1776  6227020800.0, 87178291200.0, 1307674368000.0, 20922789888000.0, 355687428096000.0, 6402373705728000.0,
1777  121645100408832000.0, 2432902008176640000.0, 51090942171709440000.0, 1124000727777607680000.0,
1778  25852016738884976640000.0, 620448401733239439360000.0, 15511210043330985984000000.0,
1779  403291461126605635584000000.0, 10888869450418352160768000000.0, 304888344611713860501504000000.0,
1780  8841761993739701954543616000000.0, 265252859812191058636308480000000.0,
1781  8222838654177922817725562880000000.0, 263130836933693530167218012160000000.0
1782  };
1783  PositionVector ret;
1784  const int npts = (int)size();
1785  // calculate the points on the Bezier curve
1786  const double step = (double) 1.0 / (numPoints - 1);
1787  double t = 0.;
1788  Position prev;
1789  for (int i1 = 0; i1 < numPoints; i1++) {
1790  if ((1.0 - t) < 5e-6) {
1791  t = 1.0;
1792  }
1793  double x = 0., y = 0., z = 0.;
1794  for (int i = 0; i < npts; i++) {
1795  const double ti = (i == 0) ? 1.0 : pow(t, i);
1796  const double tni = (npts == i + 1) ? 1.0 : pow(1 - t, npts - i - 1);
1797  const double basis = fac[npts - 1] / (fac[i] * fac[npts - 1 - i]) * ti * tni;
1798  x += basis * at(i).x();
1799  y += basis * at(i).y();
1800  z += basis * at(i).z();
1801  }
1802  t += step;
1803  Position current(x, y, z);
1804  if (prev != current && !ISNAN(x) && !ISNAN(y) && !ISNAN(z)) {
1805  ret.push_back(current);
1806  }
1807  prev = current;
1808  }
1809  return ret;
1810 }
1811 
1812 
1813 /****************************************************************************/
#define DEG2RAD(x)
Definition: GeomHelper.h:35
#define RAD2DEG(x)
Definition: GeomHelper.h:36
#define WRITE_WARNINGF(...)
Definition: MsgHandler.h:281
#define WRITE_ERROR(msg)
Definition: MsgHandler.h:288
std::ostream & operator<<(std::ostream &os, const PositionVector &geom)
bool gDebugFlag1
global utility flags for debugging
Definition: StdDefs.cpp:32
const double INVALID_DOUBLE
Definition: StdDefs.h:63
T MIN2(T a, T b)
Definition: StdDefs.h:74
T ISNAN(T a)
Definition: StdDefs.h:115
T MAX2(T a, T b)
Definition: StdDefs.h:80
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:46
virtual bool partialWithin(const AbstractPoly &poly, double offset=0) const =0
Returns whether the AbstractPoly is partially within the given polygon.
virtual bool crosses(const Position &p1, const Position &p2) const =0
Returns whether the AbstractPoly crosses the given line.
virtual bool around(const Position &p, double offset=0) const =0
Returns whether the AbstractPoly the given coordinate.
A class that stores a 2D geometrical boundary.
Definition: Boundary.h:39
void add(double x, double y, double z=0)
Makes the boundary include the given coordinate.
Definition: Boundary.cpp:77
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:82
static const double INVALID_OFFSET
a value to signify offsets outside the range of [0, Line.length()]
Definition: GeomHelper.h:50
static double nearest_offset_on_line_to_point2D(const Position &lineStart, const Position &lineEnd, const Position &p, bool perpendicular=true)
Definition: GeomHelper.cpp:88
static double legacyDegree(const double angle, const bool positive=false)
Definition: GeomHelper.cpp:215
static double angleDiff(const double angle1, const double angle2)
Returns the difference of the second angle to the first angle in radiants.
Definition: GeomHelper.cpp:179
A point in 2D or 3D with translation and scaling methods.
Definition: Position.h:37
static const Position INVALID
used to indicate that a position is valid
Definition: Position.h:293
double distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
Definition: Position.h:252
double distanceTo(const Position &p2) const
returns the euclidean distance in 3 dimension
Definition: Position.h:242
void sub(double dx, double dy)
Substracts the given position from this one.
Definition: Position.h:145
double x() const
Returns the x-position.
Definition: Position.h:55
void add(const Position &pos)
Adds the given position to this one.
Definition: Position.h:125
double z() const
Returns the z-position.
Definition: Position.h:65
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:262
bool almostSame(const Position &p2, double maxDiv=POSITION_EPS) const
check if two position is almost the sme as other
Definition: Position.h:237
double y() const
Returns the y-position.
Definition: Position.h:60
int operator()(const Position &p1, const Position &p2) const
comparing operation for sort
clase for increasing Sorter
int operator()(const Position &p1, const Position &p2) const
comparing operation
A list of positions.
PositionVector operator-(const PositionVector &v2) const
substracts two vectors (requires vectors of the same length)
void scaleAbsolute(double offset)
enlarges/shrinks the polygon by an absolute offset based at the centroid
double length2D() const
Returns the length.
void append(const PositionVector &v, double sameThreshold=2.0)
bool overlapsWith(const AbstractPoly &poly, double offset=0) const
Returns the information whether the given polygon overlaps with this.
PositionVector added(const Position &offset) const
double isLeft(const Position &P0, const Position &P1, const Position &P2) const
get left
double beginEndAngle() const
returns the angle in radians of the line connecting the first and the last position
double length() const
Returns the length.
void move2sideCustom(std::vector< double > amount, double maxExtension=100)
move position vector to side using a custom offset for each geometry point
void sortAsPolyCWByAngle()
sort as polygon CW by angle
PositionVector simplified() const
return the same shape with intermediate colinear points removed
void rotate2D(double angle)
PositionVector()
Constructor. Creates an empty position vector.
Position getPolygonCenter() const
Returns the arithmetic of all corner points.
Position intersectionPosition2D(const Position &p1, const Position &p2, const double withinDist=0.) const
Returns the position of the intersection.
const Position & operator[](int index) const
returns the constat position at the given index @ToDo !!! exceptions?
double rotationAtOffset(double pos) const
Returns the rotation at the given length.
std::vector< Position > vp
vector of position
void push_front_noDoublePos(const Position &p)
insert in front a non double position
bool operator!=(const PositionVector &v2) const
comparing operation
PositionVector resample(double maxLength, const bool adjustEnd) const
resample shape (i.e. transform to segments, equal spacing)
void sortByIncreasingXY()
sort by increasing X-Y Positions
double rotationDegreeAtOffset(double pos) const
Returns the rotation at the given length.
bool isNAN() const
check if PositionVector is NAN
Position positionAtOffset(double pos, double lateralOffset=0) const
Returns the position at the given length.
void add(double xoff, double yoff, double zoff)
void closePolygon()
ensures that the last position equals the first
static Position sideOffset(const Position &beg, const Position &end, const double amount)
get a side position of position vector using a offset
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 ...
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 prepend(const PositionVector &v, double sameThreshold=2.0)
double nearest_offset_to_point2D(const Position &p, bool perpendicular=true) const
return the nearest offest to point 2D
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
PositionVector getOrthogonal(const Position &p, double extend, bool before, double length=1.0, double deg=90) const
return orthogonal through p (extending this vector if necessary)
int indexOfClosest(const Position &p, bool twoD=false) const
std::pair< PositionVector, PositionVector > splitAt(double where, bool use2D=false) const
Returns the two lists made when this list vector is splitted at the given point.
void move2side(double amount, double maxExtension=100)
move position vector to side using certain ammount
bool crosses(const Position &p1, const Position &p2) const
Returns whether the AbstractPoly crosses the given line.
PositionVector getSubpart2D(double beginOffset, double endOffset) const
get subpart of a position vector in two dimensions (Z is ignored)
PositionVector interpolateZ(double zStart, double zEnd) const
returned vector that varies z smoothly over its length
Boundary getBoxBoundary() const
Returns a boundary enclosing this list of lines.
double offsetAtIndex2D(int index) const
return the offset at the given index
PositionVector smoothedZFront(double dist=std::numeric_limits< double >::max()) const
returned vector that is smoothed at the front (within dist)
double angleAt2D(int pos) const
get angle in certain position of position vector
void insert_noDoublePos(const std::vector< Position >::iterator &at, const Position &p)
insert in front a non double position
double slopeDegreeAtOffset(double pos) const
Returns the slope at the given length.
bool hasElevation() const
return whether two positions differ in z-coordinate
static const PositionVector EMPTY
empty Vector
void extrapolate(const double val, const bool onlyFirst=false, const bool onlyLast=false)
extrapolate position vector
PositionVector bezier(int numPoints)
return a bezier interpolation
Position getLineCenter() const
get line center
Position getCentroid() const
Returns the centroid (closes the polygon if unclosed)
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...
PositionVector operator+(const PositionVector &v2) const
adds two vectors (requires vectors of the same length)
void extrapolate2D(const double val, const bool onlyFirst=false)
extrapolate position vector in two dimensions (Z is ignored)
int insertAtClosest(const Position &p, bool interpolateZ)
inserts p between the two closest positions
void push_front(const Position &p)
insert in front a Position
void scaleRelative(double factor)
enlarges/shrinks the polygon by a factor based at the centroid
void push_back_noDoublePos(const Position &p)
insert in back a non double position
void removeDoublePoints(double minDist=POSITION_EPS, bool assertLength=false, int beginOffset=0, int endOffset=0, bool resample=false)
Removes positions if too near.
bool partialWithin(const AbstractPoly &poly, double offset=0) const
Returns the information whether this polygon lies partially within the given polygon.
double getMaxGrade(double &maxJump) const
double area() const
Returns the area (0 for non-closed)
bool isClosed() const
check if PositionVector is closed
void pop_front()
pop first Position
double nearest_offset_to_point25D(const Position &p, bool perpendicular=true) const
return the nearest offest to point 2D projected onto the 3D geometry
int removeClosest(const Position &p)
removes the point closest to p and return the removal index
static double localAngle(const Position &from, const Position &pos, const Position &to)
bool intersects(const Position &p1, const Position &p2) const
Returns the information whether this list of points interesects the given line.
PositionVector reverse() const
reverse position vector
PositionVector getSubpartByIndex(int beginIndex, int count) const
get subpart of a position vector using index and a cout
Position positionAtOffset2D(double pos, double lateralOffset=0) const
Returns the position at the given length.
bool operator==(const PositionVector &v2) const
comparing operation
void sub(const Position &offset)
PositionVector getSubpart(double beginOffset, double endOffset) const
get subpart of a position vector
~PositionVector()
Destructor.
bool around(const Position &p, double offset=0) const
Returns the information whether the position vector describes a polygon lying around the given point.
Position transformToVectorCoordinates(const Position &p, bool extend=false) const
return position p within the length-wise coordinate system defined by this position vector....
#define M_PI
Definition: odrSpiral.cpp:40