Polygon2D.cpp
Go to the documentation of this file.
1 // Polygon2D
2 //
3 //
4 
5 
6 #include <cmath>
7 
8 #include "Polygon2D.hpp"
9 
10 #include "Box2D.hpp"
11 #include "Circle2D.hpp"
12 #include "Ellipse2D.hpp"
13 #include "Line2D.hpp"
14 #include <iterator>
15 #include <sstream>
16 #include <stdexcept> // for throw
17 
18 namespace datatypes
19 {
20 
21 // ////////////////////////////////////////////////////////////
22 
24 {
25 }
26 
28 {
29  push_back(p1);
30 }
31 
32 Polygon2D::Polygon2D(const Point2D& p1, const Point2D& p2)
33 {
34  push_back(p1);
35  push_back(p2);
36 }
37 Polygon2D::Polygon2D(const Point2D& p1, const Point2D& p2, const Point2D& p3)
38 {
39  push_back(p1);
40  push_back(p2);
41  push_back(p3);
42 }
43 
44 Polygon2D::Polygon2D(const Point2D& p1, const Point2D& p2, const Point2D& p3, const Point2D& p4)
45 {
46  push_back(p1);
47  push_back(p2);
48  push_back(p3);
49  push_back(p4);
50 }
51 
52 /*
53 Polygon2D::Polygon2D(const Line2D& p)
54 {
55  push_back(p.getP1());
56  push_back(p.getP2());
57 }
58 */
59 
60 Polygon2D::Polygon2D(const base_class& other_vector)
61  : base_class(other_vector)
62 {
63 }
64 
65 /*
66 Polygon2D::Polygon2D(const std::string& polygonAsString)
67 {
68  fromString(polygonAsString);
69 }
70 */
71 
73 {
74  base_class::insert(base_class::end(), other.begin(), other.end());
75  return *this;
76 }
77 
79 {
80  push_back(point);
81  return *this;
82 }
83 
85 {
86  push_back(Point2D(x, y));
87  return *this;
88 }
89 
90 double Polygon2D::getArea() const
91 {
92  if (empty())
93  return 0.0;
94 
95  double result(0.0);
96  for (size_type i = 0; i < size() - 1; ++i)
97  {
98  result += at(i).getX() * at(i + 1).getY() - at(i + 1).getX() * at(i).getY();
99  }
100 
101  // Automatically close polygon. This won't harm, if polygon is already closed.
102  result += at(size() - 1).getX() * at(0).getY() - at(0).getX() * at(size() - 1).getY();
103 
104  // Calculate absolute value in order to work with counter-clockwise polygon description.
105  return std::abs(0.5 * result);
106 }
107 
109 {
110  Point2D centerOfGravity;
111 
112  if (empty())
113  {
114  return centerOfGravity;
115  }
116 
117  for (Polygon2D::const_iterator edgeIter = begin(); edgeIter != end(); ++edgeIter)
118  {
119  centerOfGravity += *edgeIter;
120  }
121 
122  centerOfGravity /= size();
123 
124  return centerOfGravity;
125 }
126 
128 {
129  return !empty() && (front() == back());
130 }
131 
133 //
134 // static functions
135 //
137 
138 Polygon2D Polygon2D::fromCircle( const Point2D& center, const floatingpoint_type radius, const UINT32 samplingPoints )
139 {
140  const Circle2D circle(center, radius);
141  return fromEllipse(circle, samplingPoints);
142 }
143 
144 Polygon2D Polygon2D::fromEllipse( const Point2D& center, const floatingpoint_type a, const floatingpoint_type b, const floatingpoint_type angle, const UINT32 samplingPoints )
145 {
146  const Ellipse2D ellipse(center, Point2D(a, b), angle);
147  return fromEllipse(ellipse, samplingPoints);
148 }
149 
150 Polygon2D Polygon2D::fromEllipse(const Ellipse2D& ellipse, const UINT32 samplingPoints)
151 {
152  Polygon2D result = fromArc(ellipse, 0, 2.0 * M_PI, samplingPoints);
153 
154  // Since we know the result should be a closed circle, we can
155  // force the final point to be identical to the first one here.
156  if (result.size() > 1)
157  result.back() = result.front();
158 
159  return result;
160 }
161 
163  const floatingpoint_type startAngle, const floatingpoint_type endAngle,
164  const UINT32 samplingPoints, const bool clockwise)
165 {
166  floatingpoint_type end = endAngle;
167 
168  // Modify the end angle so that we get the expected direction
169  if (clockwise == true)
170  {
171  // clockwise direction (descending angle values from start to end)
172  while (startAngle < end)
173  {
174  end -= 2.0 * PI;
175  }
176  }
177  else
178  {
179  // counter-clockwise direction (ascending angle values from start to end)
180  while (end < startAngle)
181  {
182  end += 2.0 * PI;
183  }
184  }
185 
186  return fromArc(ellipse, startAngle, end, samplingPoints);
187 }
188 
190  const floatingpoint_type startAngle, const floatingpoint_type endAngle,
191  const UINT32 samplingPoints)
192 {
193  Polygon2D polygon;
194 
195  // Check if we have at least 2 sampling points
196  if (samplingPoints < 2)
197  {
198  // Not enough points! Return an empty output.
199  return polygon;
200  }
201 
202  if (fabs(endAngle - startAngle) > double(2.0 * M_PI + 1e-3)) // allow 10^-3 epsilon
203  {
204  throw std::invalid_argument("Polygon2D::fromArc: Angle difference is too large! Is " + ::toString(fabs(endAngle - startAngle), 2) + " but must be at most 2*pi.");
205  }
206 
207  const floatingpoint_type sinAngle(std::sin(ellipse.getRotation()));
208  const floatingpoint_type cosAngle(std::cos(ellipse.getRotation()));
209  const floatingpoint_type step = (endAngle - startAngle) / floatingpoint_type(samplingPoints - 1);
210 
211  floatingpoint_type t = startAngle;
212  for (UINT32 p = 0 ; p < samplingPoints; p++)
213  {
214  // Last point? Then force the angle exactly on the endAngle
215  if (p == (samplingPoints - 1))
216  {
217  t = endAngle;
218  }
219 
220  Point2D point;
221  point.setX(ellipse.getRadius().getX() * std::cos(t) * cosAngle - ellipse.getRadius().getY() * std::sin(t) * sinAngle);
222  point.setY(ellipse.getRadius().getX() * std::cos(t) * sinAngle + ellipse.getRadius().getY() * std::sin(t) * cosAngle);
223  point += ellipse.getCenter();
224  polygon.push_back(point);
225  t += step;
226  }
227 
228  return polygon;
229 }
230 
232 {
233  return "()[],; \t\"'";
234 }
235 
237 {
238  Polygon2D contour;
239  contour.push_back(Point2D(center.getX() - radius, center.getY()));
240  contour.push_back(Point2D(center.getX(), center.getY() - radius));
241  contour.push_back(Point2D(center.getX() + radius, center.getY()));
242  contour.push_back(Point2D(center.getX(), center.getY() + radius));
243  contour.push_back(Point2D(center.getX() - radius, center.getY()));
244 
245  return contour;
246 }
247 
248 Polygon2D Polygon2D::createRectangle(const Point2D& lowerLeft, const Point2D& upperRight)
249 {
250  return Polygon2D()
251  .append(lowerLeft)
252  .append(Point2D(lowerLeft.getX(), upperRight.getY()))
253  .append(upperRight)
254  .append(Point2D(upperRight.getX(), lowerLeft.getY()));
255 }
256 
257 /*
258 void Polygon2D::fromString(const std::string& polyString)
259 {
260  clear();
261 
262  typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
263  tokenizer listOfTokens(polyString, boost::char_separator<char>(getSeparatorCharacters()));
264  for (tokenizer::const_iterator iter = listOfTokens.begin();
265  iter != listOfTokens.end(); ++iter)
266  {
267  floatingpoint_type x = boost::lexical_cast<floatingpoint_type>(*iter);
268  ++iter;
269  if (iter == listOfTokens.end())
270  throw std::runtime_error("When parsing a Polygon2D string, the last point only has a x component.");
271  floatingpoint_type y = boost::lexical_cast<floatingpoint_type>(*iter);
272  this->push_back(Point2D(x, y));
273  }
274 }
275 */
276 
278 {
279  if (empty())
280  {
281  return Box2D();
282  }
283 
284  Polygon2D::const_iterator iter = begin();
285  floatingpoint_type minX = iter->getX();
286  floatingpoint_type minY = iter->getY();
287  floatingpoint_type maxX = minX;
288  floatingpoint_type maxY = minY;
289 
290  ++iter;
291  for ( ; iter != end(); ++iter)
292  {
293  const floatingpoint_type x = iter->getX();
294  const floatingpoint_type y = iter->getY();
295  if (x < minX) minX = x;
296  if (x > maxX) maxX = x;
297  if (y < minY) minY = y;
298  if (y > maxY) maxY = y;
299  }
300  return Box2D(0.5 * (maxX + minX),
301  0.5 * (maxY + minY),
302  maxX - minX,
303  maxY - minY);
304 }
305 
306 std::pair<Polygon2D::floatingpoint_type, Polygon2D::floatingpoint_type> Polygon2D::getBoundingAngles() const
307 {
308  if (empty())
309  return std::make_pair(0, 0);
310 
311  // Need to check whether the origin is inside the polygon. BUT: If
312  // the origin is directly on the boundary, containsPoint() may or
313  // may not return true. For this reason we must watch out for the
314  // case of points on the negative x axis below (if this test
315  // returned false here).
316  if (containsPoint(Point2D(0, 0)))
317  {
318  // Origin is inside. Then return the full interval.
319  return std::make_pair(-PI, PI);
320  }
321 
322  // The usual case: The polygon does not contain the origin. Then we
323  // look for the min and max angles of the edges.
324 
325  Polygon2D::const_iterator iter = begin();
326  floatingpoint_type minangle = iter->angle();
327  floatingpoint_type maxangle = minangle;
328 
329  bool gotPointOnNegativeX = false;
330  for ( ; iter != end(); ++iter)
331  {
332  if (fuzzyCompare(iter->getY(), 0.0)
333  && (iter->getX() < 0.0))
334  {
335  // Special case for points on the negative x axis: We do
336  // not know (yet) whether we should record this as +pi or
337  // -pi. We will decide on this afterwards.
338  gotPointOnNegativeX = true;
339  }
340  else
341  {
342  const floatingpoint_type pointangle = iter->angle();
343 
344  if (pointangle < minangle)
345  minangle = pointangle;
346 
347  if (pointangle > maxangle)
348  maxangle = pointangle;
349  }
350  }
351 
352  // Did we also have at least one point on the negative X axis?
353  // Either add this as -pi or +pi, or set both angles to -pi/pi
354  // because we are not sure.
355  if (gotPointOnNegativeX)
356  {
357  if (std::max(minangle, maxangle) <= 0.0f)
358  minangle = -M_PI;
359  else if (std::min(minangle, maxangle) >= 0.0f)
360  maxangle = M_PI;
361  else
362  {
363  minangle = -M_PI;
364  maxangle = M_PI;
365  }
366  }
367 
368  return std::make_pair(minangle, maxangle);
369 }
370 
420 bool Polygon2D::containsPoint(const Point2D& point) const
421 {
422  // Point-in-polygon test using ray-casting algorithm
423  //
424  // C-Code was borrowed from:
425  // http://www.ecse.rpi.edu/Homepages/wrf/Research/Short_Notes/pnpoly.html
426  //
427  // Be careful: Don't fiddle with the code below unless you know
428  // exactly what you're doing!
429  //
430  // Algorithm: From the test point a semi-finite ray is run through
431  // the polygon. While doing so the number of edge crossings are
432  // counted. If the number of crossings is even, the test point is
433  // located outside of the polygon. If the number is odd, the point
434  // is inside. (Jordan curve theorem)
435  //
436  // Also see: http://en.wikipedia.org/wiki/Point_in_polygon
437 
438  if (empty())
439  {
440  return false;
441  }
442 
443  bool isInside(false);
444  const Polygon2D& p = *this; // alias for shorter writing
445 
446  UINT32 i = 0;
447  UINT32 j = (UINT32)(size() - 1);
448  for (i = 0; i < size(); j = i++)
449  {
450  if (
451  ( (p[i].getY() > point.getY()) != (p[j].getY() > point.getY()) )
452  &&
453  ( point.getX() < ( (p[j].getX() - p[i].getX())
454  * (point.getY() - p[i].getY())
455  / (p[j].getY() - p[i].getY())
456  + p[i].getX() ) )
457  )
458  {
459  isInside = !isInside;
460  }
461  }
462 
463  return isInside;
464 }
465 
466 void projectOntoAxis(const Point2D& axis, const Polygon2D& polygon,
468 {
469  min = max = (axis * polygon[0]);
470 
471  for (Polygon2D::const_iterator edge = polygon.begin(); edge != polygon.end(); ++edge)
472  {
473  const Polygon2D::floatingpoint_type dotProduct(axis * (*edge));
474  min = std::min(min, dotProduct);
475  max = std::max(max, dotProduct);
476  }
477 }
478 
481 {
482  if (minA < minB)
483  {
484  return minB - maxA;
485  }
486  else
487  {
488  return minA - maxB;
489  }
490 }
491 
492 bool Polygon2D::isColliding(const Polygon2D& p2) const
493 {
494  Polygon2D both(*this);
495  both.reserve(both.size() + p2.size());
496  both.insert(both.end(), p2.begin(), p2.end());
497 
498  for (Polygon2D::const_iterator edge = both.begin(); edge != both.end(); ++edge)
499  {
500  Point2D axis(-edge->getY(), edge->getX());
501  axis.normalize();
502 
503  floatingpoint_type minA, minB, maxA, maxB;
504  projectOntoAxis(axis, *this, minA, maxA);
505  projectOntoAxis(axis, p2, minB, maxB);
506 
507  if (intervalDistance(minA, maxA, minB, maxB) > 0)
508  {
509  return false;
510  }
511  }
512 
513  return true;
514 }
515 
516 
518 {
519  base_class result;
520  if (size() < 2 || other.isZero())
521  {
522  return result;
523  }
524  for (size_t k = 1; k < size(); ++k)
525  {
526  Point2D isect;
527  Line2D currentline(operator[](k - 1), operator[](k));
528  if (currentline.isIntersecting(other, &isect)
530  result.push_back(isect);
531  }
532  return result;
533 }
534 
536 {
537  if (size() < 2)
538  return empty() ? 0 : point.dist(front());
539 
540  assert(size() >= 2);
541  Point2D::value_type result = point.dist(front());
542  for (size_t k = 1; k < size(); ++k)
543  {
544  Line2D line(operator[](k - 1), operator[](k));
545  Point2D::value_type dist = line.distanceFromLineSegment(point);
546  if (dist < result)
547  result = dist;
548  }
549  return result;
550 }
551 
553 {
554  if (size() < 3)
555  return *this;
556  Polygon2D result;
557  result.reserve(size());
558 
559  // First point is always in the result
560  result.push_back(front());
561 
562  for (size_type k = 1; k < size() - 1; ++k)
563  {
564  const Point2D& previous = result.back();
565  const Point2D& current = operator[](k);
566  const Point2D& next = operator[](k + 1);
567  Line2D line(previous, next);
568  // Only add the current point to the result if it doesn't lie
569  // directly on the connection line between the previous output
570  // point and the next one.
571  if (!line.containsPoint(current))
572  result.push_back(current);
573  }
574 
575  // Last point is always in the result
576  result.push_back(back());
577 
578  return result;
579 }
580 
581 // ////////////////////////////////////////////////////////////
582 
583 
584 std::string Polygon2D::toString() const
585 {
586  std::string text = "[ ";
587  if (empty() == false)
588  {
589  for (size_type k = 0; k < size(); k++)
590  {
591  const Point2D& current = operator[](k);
592  text += current.toString() + " ";
593  }
594  }
595  text += "]";
596  return text;
597 }
598 
599 } // namespace datatypes
std::string toString(UINT16 digits=2) const
Text output for debugging.
Definition: Point2D.cpp:75
bool isColliding(const Polygon2D &p2) const
Returns true if the given polygon collides with this polygon.
Definition: Polygon2D.cpp:492
const Point2D & getCenter() const
Returns the center point of this Ellipse.
Definition: Ellipse2D.hpp:88
static Polygon2D createRectangle(const Point2D &lowerLeft, const Point2D &upperRight)
Definition: Polygon2D.cpp:248
std::string toString() const
Text output for debugging.
Definition: Polygon2D.cpp:584
value_type getX() const
Definition: Point2D.hpp:70
bool containsPoint(const Point2D &point) const
Returns true if the given Point2D is inside this polygon.
Definition: Polygon2D.cpp:420
A rotated 2-dimensional box in the plane.
Definition: Box2D.hpp:34
Point2D getCenterOfGravity() const
Returns the center of gravity of this polygon.
Definition: Polygon2D.cpp:108
Box2D getBoundingBox() const
Returns a Box in parallel to the coordinate system that bounds this polygon.
Definition: Polygon2D.cpp:277
Polygon2D getSimplified() const
Returns a polygon with potentially less edge points.
Definition: Polygon2D.cpp:552
uint32_t UINT32
Point2D::value_type floatingpoint_type
Definition: Polygon2D.hpp:52
A polygon of 2D-points.
Definition: Polygon2D.hpp:43
Polygon2D()
Constructor for an empty polygon.
Definition: Polygon2D.cpp:23
std::pair< floatingpoint_type, floatingpoint_type > getBoundingAngles() const
Definition: Polygon2D.cpp:306
value_type getRotation() const
Definition: Ellipse2D.hpp:104
void setX(value_type x)
Definition: Point2D.hpp:140
value_type dist() const
Definition: Point2D.hpp:353
value_type getY() const
Definition: Point2D.hpp:73
static Polygon2D rhombus(const Point2D &center, const floatingpoint_type radius)
Static function to create a rhombus.
Definition: Polygon2D.cpp:236
bool containsPoint(const Point2D &point) const
Returns true if this line "contains" the given point.
Definition: Line2D.cpp:75
Polygon2D & append(const Polygon2D &other)
Appends the points of the other polygon to this polygon.
Definition: Polygon2D.cpp:72
Polygon2D::floatingpoint_type intervalDistance(const Polygon2D::floatingpoint_type minA, const Polygon2D::floatingpoint_type maxA, const Polygon2D::floatingpoint_type minB, const Polygon2D::floatingpoint_type maxB)
Definition: Polygon2D.cpp:479
bool isClosed() const
Returns true if this is explicitly a closed polygon.
Definition: Polygon2D.cpp:127
static Polygon2D fromArc(const Ellipse2D &ellipse, const floatingpoint_type startAngle, const floatingpoint_type endAngle, const UINT32 samplingPoints, const bool clockwise)
(DEPRECATED) Create a Polygon2D approximation of the arc of an ellipse
Definition: Polygon2D.cpp:162
void projectOntoAxis(const Point2D &axis, const Polygon2D &polygon, Polygon2D::floatingpoint_type &min, Polygon2D::floatingpoint_type &max)
Definition: Polygon2D.cpp:466
base_class isIntersecting(const Line2D &other) const
Calculates all intersection points between a line and this polygon.
Definition: Polygon2D.cpp:517
A line in the two-dimensional plane, composed out of two points.
Definition: Line2D.hpp:24
double value_type
The type of the stored x and y coordinates.
Definition: Point2D.hpp:31
double distanceFromLineSegment(const Point2D &point) const
Returns the distance of a point to this line segment.
Definition: Line2D.cpp:110
bool fuzzyCompare(double a, double b)
Tests if two double values are nearly equal.
Definition: MathToolbox.hpp:28
static Polygon2D fromEllipse(const Point2D &center, const floatingpoint_type a, const floatingpoint_type b, const floatingpoint_type angle, const UINT32 samplingPoints=32)
Static function to create a Polygon2D from ellipse parameters.
Definition: Polygon2D.cpp:144
static Polygon2D fromCircle(const Point2D &center, const floatingpoint_type radius, const UINT32 samplingPoints=32)
Static function to get Polygon2D from circle parameters.
Definition: Polygon2D.cpp:138
A rotated 2-dimensional ellipse in the plane.
Definition: Ellipse2D.hpp:30
static const char * getSeparatorCharacters()
Definition: Polygon2D.cpp:231
bool isZero() const
Returns true if both points are zero.
Definition: Line2D.hpp:80
void setY(value_type y)
Definition: Point2D.hpp:143
IntersectingType isIntersecting(const Line2D &other, Point2D *intersectingPoint=NULL) const
Calculates the intersection point between two lines.
Definition: Line2D.cpp:258
Point2D::value_type distanceToPoint(const Point2D &point) const
Returns the distance of a point to this polyon.
Definition: Polygon2D.cpp:535
The lines are intersecting within their line segments.
Definition: Line2D.hpp:38
std::vector< Point2D > base_class
The base type. (Naming according to boost convention.)
Definition: Polygon2D.hpp:48
const Point2D & getRadius() const
Returns the radius of this Ellipse.
Definition: Ellipse2D.hpp:97
A circle in the plane.
Definition: Circle2D.hpp:21
double getArea() const
Returns the area enclosed by this polygon.
Definition: Polygon2D.cpp:90
#define PI


libsick_ldmrs
Author(s): SICK AG , Martin Günther , Jochen Sprickerhof
autogenerated on Sat Jun 8 2019 17:57:33