Polygon2D.cpp
Go to the documentation of this file.
00001 // Polygon2D
00002 //
00003 //
00004 
00005 
00006 #include <cmath>
00007 
00008 #include "Polygon2D.hpp"
00009 
00010 #include "Box2D.hpp"
00011 #include "Circle2D.hpp"
00012 #include "Ellipse2D.hpp"
00013 #include "Line2D.hpp"
00014 #include <iterator>
00015 #include <sstream>
00016 #include <stdexcept>    // for throw
00017 
00018 namespace datatypes
00019 {
00020 
00021 // ////////////////////////////////////////////////////////////
00022 
00023 Polygon2D::Polygon2D()
00024 {
00025 }
00026 
00027 Polygon2D::Polygon2D(const Point2D& p1)
00028 {
00029         push_back(p1);
00030 }
00031 
00032 Polygon2D::Polygon2D(const Point2D& p1, const Point2D& p2)
00033 {
00034         push_back(p1);
00035         push_back(p2);
00036 }
00037 Polygon2D::Polygon2D(const Point2D& p1, const Point2D& p2, const Point2D& p3)
00038 {
00039         push_back(p1);
00040         push_back(p2);
00041         push_back(p3);
00042 }
00043 
00044 Polygon2D::Polygon2D(const Point2D& p1, const Point2D& p2, const Point2D& p3, const Point2D& p4)
00045 {
00046         push_back(p1);
00047         push_back(p2);
00048         push_back(p3);
00049         push_back(p4);
00050 }
00051 
00052 /*
00053 Polygon2D::Polygon2D(const Line2D& p)
00054 {
00055         push_back(p.getP1());
00056         push_back(p.getP2());
00057 }
00058 */
00059 
00060 Polygon2D::Polygon2D(const base_class& other_vector)
00061         : base_class(other_vector)
00062 {
00063 }
00064 
00065 /*
00066 Polygon2D::Polygon2D(const std::string& polygonAsString)
00067 {
00068         fromString(polygonAsString);
00069 }
00070 */
00071 
00072 Polygon2D& Polygon2D::append (const Polygon2D& other )
00073 {
00074         base_class::insert(base_class::end(), other.begin(), other.end());
00075         return *this;
00076 }
00077 
00078 Polygon2D& Polygon2D::append(const Point2D& point)
00079 {
00080         push_back(point);
00081         return *this;
00082 }
00083 
00084 Polygon2D& Polygon2D::append(floatingpoint_type x, floatingpoint_type y)
00085 {
00086         push_back(Point2D(x, y));
00087         return *this;
00088 }
00089 
00090 double Polygon2D::getArea() const
00091 {
00092         if (empty())
00093                 return 0.0;
00094 
00095         double result(0.0);
00096         for (size_type i = 0; i < size() - 1; ++i)
00097         {
00098                 result += at(i).getX() * at(i + 1).getY() - at(i + 1).getX() * at(i).getY();
00099         }
00100 
00101         // Automatically close polygon. This won't harm, if polygon is already closed.
00102         result += at(size() - 1).getX() * at(0).getY() - at(0).getX() * at(size() - 1).getY();
00103 
00104         // Calculate absolute value in order to work with counter-clockwise polygon description.
00105         return std::abs(0.5 * result);
00106 }
00107 
00108 Point2D Polygon2D::getCenterOfGravity() const
00109 {
00110         Point2D centerOfGravity;
00111 
00112         if (empty())
00113         {
00114                 return centerOfGravity;
00115         }
00116 
00117         for (Polygon2D::const_iterator edgeIter = begin(); edgeIter != end(); ++edgeIter)
00118         {
00119                 centerOfGravity += *edgeIter;
00120         }
00121 
00122         centerOfGravity /= size();
00123 
00124         return centerOfGravity;
00125 }
00126 
00127 bool Polygon2D::isClosed() const
00128 {
00129         return !empty() && (front() == back());
00130 }
00131 
00133 //
00134 // static functions
00135 //
00137 
00138 Polygon2D Polygon2D::fromCircle( const Point2D& center, const floatingpoint_type radius, const UINT32 samplingPoints )
00139 {
00140         const Circle2D circle(center, radius);
00141         return fromEllipse(circle, samplingPoints);
00142 }
00143 
00144 Polygon2D Polygon2D::fromEllipse( const Point2D& center, const floatingpoint_type a, const floatingpoint_type b, const floatingpoint_type angle, const UINT32 samplingPoints )
00145 {
00146         const Ellipse2D ellipse(center, Point2D(a, b), angle);
00147         return fromEllipse(ellipse, samplingPoints);
00148 }
00149 
00150 Polygon2D Polygon2D::fromEllipse(const Ellipse2D& ellipse, const UINT32 samplingPoints)
00151 {
00152         Polygon2D result = fromArc(ellipse, 0, 2.0 * M_PI, samplingPoints);
00153 
00154         // Since we know the result should be a closed circle, we can
00155         // force the final point to be identical to the first one here.
00156         if (result.size() > 1)
00157                 result.back() = result.front();
00158 
00159         return result;
00160 }
00161 
00162 Polygon2D Polygon2D::fromArc(const Ellipse2D& ellipse,
00163                                                          const floatingpoint_type startAngle, const floatingpoint_type endAngle,
00164                                                          const UINT32 samplingPoints, const bool clockwise)
00165 {
00166         floatingpoint_type end = endAngle;
00167 
00168         // Modify the end angle so that we get the expected direction
00169         if (clockwise == true)
00170         {
00171                 // clockwise direction (descending angle values from start to end)
00172                 while (startAngle < end)
00173                 {
00174                         end -= 2.0 * PI;
00175                 }
00176         }
00177         else
00178         {
00179                 // counter-clockwise direction (ascending angle values from start to end)
00180                 while (end < startAngle)
00181                 {
00182                         end += 2.0 * PI;
00183                 }
00184         }
00185 
00186         return fromArc(ellipse, startAngle, end, samplingPoints);
00187 }
00188 
00189 Polygon2D Polygon2D::fromArc(const Ellipse2D& ellipse,
00190                                                          const floatingpoint_type startAngle, const floatingpoint_type endAngle,
00191                                                          const UINT32 samplingPoints)
00192 {
00193         Polygon2D polygon;
00194 
00195         // Check if we have at least 2 sampling points
00196         if (samplingPoints < 2)
00197         {
00198                 // Not enough points! Return an empty output.
00199                 return polygon;
00200         }
00201 
00202         if (fabs(endAngle - startAngle) > double(2.0 * M_PI + 1e-3)) // allow 10^-3 epsilon
00203         {
00204                 throw std::invalid_argument("Polygon2D::fromArc: Angle difference is too large! Is " + ::toString(fabs(endAngle - startAngle), 2) + " but must be at most 2*pi.");
00205         }
00206 
00207         const floatingpoint_type sinAngle(std::sin(ellipse.getRotation()));
00208         const floatingpoint_type cosAngle(std::cos(ellipse.getRotation()));
00209         const floatingpoint_type step = (endAngle - startAngle) / floatingpoint_type(samplingPoints - 1);
00210 
00211         floatingpoint_type t = startAngle;
00212         for (UINT32 p = 0 ; p < samplingPoints; p++)
00213         {
00214                 // Last point? Then force the angle exactly on the endAngle
00215                 if (p == (samplingPoints - 1))
00216                 {
00217                         t = endAngle;
00218                 }
00219 
00220                 Point2D point;
00221                 point.setX(ellipse.getRadius().getX() * std::cos(t) * cosAngle - ellipse.getRadius().getY() * std::sin(t) * sinAngle);
00222                 point.setY(ellipse.getRadius().getX() * std::cos(t) * sinAngle + ellipse.getRadius().getY() * std::sin(t) * cosAngle);
00223                 point += ellipse.getCenter();
00224                 polygon.push_back(point);
00225                 t += step;
00226         }
00227 
00228         return polygon;
00229 }
00230 
00231 const char* Polygon2D::getSeparatorCharacters()
00232 {
00233         return "()[],; \t\"'";
00234 }
00235 
00236 Polygon2D Polygon2D::rhombus(const Point2D& center, const floatingpoint_type radius)
00237 {
00238         Polygon2D contour;
00239         contour.push_back(Point2D(center.getX() - radius, center.getY()));
00240         contour.push_back(Point2D(center.getX(), center.getY() - radius));
00241         contour.push_back(Point2D(center.getX() + radius, center.getY()));
00242         contour.push_back(Point2D(center.getX(), center.getY() + radius));
00243         contour.push_back(Point2D(center.getX() - radius, center.getY()));
00244 
00245         return contour;
00246 }
00247 
00248 Polygon2D Polygon2D::createRectangle(const Point2D& lowerLeft, const Point2D& upperRight)
00249 {
00250         return Polygon2D()
00251                    .append(lowerLeft)
00252                    .append(Point2D(lowerLeft.getX(), upperRight.getY()))
00253                    .append(upperRight)
00254                    .append(Point2D(upperRight.getX(), lowerLeft.getY()));
00255 }
00256 
00257 /*
00258 void Polygon2D::fromString(const std::string& polyString)
00259 {
00260         clear();
00261 
00262         typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
00263         tokenizer listOfTokens(polyString, boost::char_separator<char>(getSeparatorCharacters()));
00264         for (tokenizer::const_iterator iter = listOfTokens.begin();
00265                  iter != listOfTokens.end(); ++iter)
00266         {
00267                 floatingpoint_type x = boost::lexical_cast<floatingpoint_type>(*iter);
00268                 ++iter;
00269                 if (iter == listOfTokens.end())
00270                         throw std::runtime_error("When parsing a Polygon2D string, the last point only has a x component.");
00271                 floatingpoint_type y = boost::lexical_cast<floatingpoint_type>(*iter);
00272                 this->push_back(Point2D(x, y));
00273         }
00274 }
00275 */
00276 
00277 Box2D Polygon2D::getBoundingBox() const
00278 {
00279         if (empty())
00280         {
00281                 return Box2D();
00282         }
00283 
00284         Polygon2D::const_iterator iter = begin();
00285         floatingpoint_type minX = iter->getX();
00286         floatingpoint_type minY = iter->getY();
00287         floatingpoint_type maxX = minX;
00288         floatingpoint_type maxY = minY;
00289 
00290         ++iter;
00291         for ( ; iter != end(); ++iter)
00292         {
00293                 const floatingpoint_type x = iter->getX();
00294                 const floatingpoint_type y = iter->getY();
00295                 if (x < minX) minX = x;
00296                 if (x > maxX) maxX = x;
00297                 if (y < minY) minY = y;
00298                 if (y > maxY) maxY = y;
00299         }
00300         return Box2D(0.5 * (maxX + minX),
00301                                  0.5 * (maxY + minY),
00302                                  maxX - minX,
00303                                  maxY - minY);
00304 }
00305 
00306 std::pair<Polygon2D::floatingpoint_type, Polygon2D::floatingpoint_type> Polygon2D::getBoundingAngles() const
00307 {
00308         if (empty())
00309                 return std::make_pair(0, 0);
00310 
00311         // Need to check whether the origin is inside the polygon. BUT: If
00312         // the origin is directly on the boundary, containsPoint() may or
00313         // may not return true. For this reason we must watch out for the
00314         // case of points on the negative x axis below (if this test
00315         // returned false here).
00316         if (containsPoint(Point2D(0, 0)))
00317         {
00318                 // Origin is inside. Then return the full interval.
00319                 return std::make_pair(-PI, PI);
00320         }
00321 
00322         // The usual case: The polygon does not contain the origin. Then we
00323         // look for the min and max angles of the edges.
00324 
00325         Polygon2D::const_iterator iter = begin();
00326         floatingpoint_type minangle = iter->angle();
00327         floatingpoint_type maxangle = minangle;
00328 
00329         bool gotPointOnNegativeX = false;
00330         for ( ; iter != end(); ++iter)
00331         {
00332                 if (fuzzyCompare(iter->getY(), 0.0)
00333                         && (iter->getX() < 0.0))
00334                 {
00335                         // Special case for points on the negative x axis: We do
00336                         // not know (yet) whether we should record this as +pi or
00337                         // -pi. We will decide on this afterwards.
00338                         gotPointOnNegativeX = true;
00339                 }
00340                 else
00341                 {
00342                         const floatingpoint_type pointangle = iter->angle();
00343 
00344                         if (pointangle < minangle)
00345                                 minangle = pointangle;
00346 
00347                         if (pointangle > maxangle)
00348                                 maxangle = pointangle;
00349                 }
00350         }
00351 
00352         // Did we also have at least one point on the negative X axis?
00353         // Either add this as -pi or +pi, or set both angles to -pi/pi
00354         // because we are not sure.
00355         if (gotPointOnNegativeX)
00356         {
00357                 if (std::max(minangle, maxangle) <= 0.0f)
00358                         minangle = -M_PI;
00359                 else if (std::min(minangle, maxangle) >= 0.0f)
00360                         maxangle = M_PI;
00361                 else
00362                 {
00363                         minangle = -M_PI;
00364                         maxangle = M_PI;
00365                 }
00366         }
00367 
00368         return std::make_pair(minangle, maxangle);
00369 }
00370 
00420 bool Polygon2D::containsPoint(const Point2D& point) const
00421 {
00422         // Point-in-polygon test using ray-casting algorithm
00423         //
00424         // C-Code was borrowed from:
00425         // http://www.ecse.rpi.edu/Homepages/wrf/Research/Short_Notes/pnpoly.html
00426         //
00427         // Be careful: Don't fiddle with the code below unless you know
00428         // exactly what you're doing!
00429         //
00430         // Algorithm: From the test point a semi-finite ray is run through
00431         // the polygon. While doing so the number of edge crossings are
00432         // counted. If the number of crossings is even, the test point is
00433         // located outside of the polygon. If the number is odd, the point
00434         // is inside. (Jordan curve theorem)
00435         //
00436         // Also see: http://en.wikipedia.org/wiki/Point_in_polygon
00437 
00438         if (empty())
00439         {
00440                 return false;
00441         }
00442 
00443         bool isInside(false);
00444         const Polygon2D& p = *this; // alias for shorter writing
00445 
00446         UINT32 i = 0;
00447         UINT32 j = (UINT32)(size() - 1);
00448         for (i = 0; i < size(); j = i++)
00449         {
00450                 if (
00451                         ( (p[i].getY() > point.getY()) != (p[j].getY() > point.getY()) )
00452                         &&
00453                         ( point.getX() < ( (p[j].getX() - p[i].getX())
00454                                                            * (point.getY() - p[i].getY())
00455                                                            / (p[j].getY() - p[i].getY())
00456                                                            + p[i].getX() ) )
00457                 )
00458                 {
00459                         isInside = !isInside;
00460                 }
00461         }
00462 
00463         return isInside;
00464 }
00465 
00466 void projectOntoAxis(const Point2D& axis, const Polygon2D& polygon,
00467                                          Polygon2D::floatingpoint_type& min, Polygon2D::floatingpoint_type& max)
00468 {
00469         min = max = (axis * polygon[0]);
00470 
00471         for (Polygon2D::const_iterator edge = polygon.begin(); edge != polygon.end(); ++edge)
00472         {
00473                 const Polygon2D::floatingpoint_type dotProduct(axis * (*edge));
00474                 min = std::min(min, dotProduct);
00475                 max = std::max(max, dotProduct);
00476         }
00477 }
00478 
00479 Polygon2D::floatingpoint_type intervalDistance(const Polygon2D::floatingpoint_type minA, const Polygon2D::floatingpoint_type maxA,
00480                 const Polygon2D::floatingpoint_type minB, const Polygon2D::floatingpoint_type maxB)
00481 {
00482         if (minA < minB)
00483         {
00484                 return minB - maxA;
00485         }
00486         else
00487         {
00488                 return minA - maxB;
00489         }
00490 }
00491 
00492 bool Polygon2D::isColliding(const Polygon2D& p2) const
00493 {
00494         Polygon2D both(*this);
00495         both.reserve(both.size() + p2.size());
00496         both.insert(both.end(), p2.begin(), p2.end());
00497 
00498         for (Polygon2D::const_iterator edge = both.begin(); edge != both.end(); ++edge)
00499         {
00500                 Point2D axis(-edge->getY(), edge->getX());
00501                 axis.normalize();
00502 
00503                 floatingpoint_type minA, minB, maxA, maxB;
00504                 projectOntoAxis(axis, *this, minA, maxA);
00505                 projectOntoAxis(axis, p2, minB, maxB);
00506 
00507                 if (intervalDistance(minA, maxA, minB, maxB) > 0)
00508                 {
00509                         return false;
00510                 }
00511         }
00512 
00513         return true;
00514 }
00515 
00516 
00517 Polygon2D::base_class Polygon2D::isIntersecting(const Line2D& other) const
00518 {
00519         base_class result;
00520         if (size() < 2 || other.isZero())
00521         {
00522                 return result;
00523         }
00524         for (size_t k = 1; k < size(); ++k)
00525         {
00526                 Point2D isect;
00527                 Line2D currentline(operator[](k - 1), operator[](k));
00528                 if (currentline.isIntersecting(other, &isect)
00529                         == Line2D::LineIntersecting)
00530                         result.push_back(isect);
00531         }
00532         return result;
00533 }
00534 
00535 Point2D::value_type Polygon2D::distanceToPoint(const Point2D& point) const
00536 {
00537         if (size() < 2)
00538                 return empty() ? 0 : point.dist(front());
00539 
00540         assert(size() >= 2);
00541         Point2D::value_type result = point.dist(front());
00542         for (size_t k = 1; k < size(); ++k)
00543         {
00544                 Line2D line(operator[](k - 1), operator[](k));
00545                 Point2D::value_type dist = line.distanceFromLineSegment(point);
00546                 if (dist < result)
00547                         result = dist;
00548         }
00549         return result;
00550 }
00551 
00552 Polygon2D Polygon2D::getSimplified() const
00553 {
00554         if (size() < 3)
00555                 return *this;
00556         Polygon2D result;
00557         result.reserve(size());
00558 
00559         // First point is always in the result
00560         result.push_back(front());
00561 
00562         for (size_type k = 1; k < size() - 1; ++k)
00563         {
00564                 const Point2D& previous = result.back();
00565                 const Point2D& current = operator[](k);
00566                 const Point2D& next = operator[](k + 1);
00567                 Line2D line(previous, next);
00568                 // Only add the current point to the result if it doesn't lie
00569                 // directly on the connection line between the previous output
00570                 // point and the next one.
00571                 if (!line.containsPoint(current))
00572                         result.push_back(current);
00573         }
00574 
00575         // Last point is always in the result
00576         result.push_back(back());
00577 
00578         return result;
00579 }
00580 
00581 // ////////////////////////////////////////////////////////////
00582 
00583 
00584 std::string Polygon2D::toString() const
00585 {
00586         std::string text = "[ ";
00587         if (empty() == false)
00588         {
00589                 for (size_type k = 0; k < size(); k++)
00590                 {
00591                         const Point2D& current = operator[](k);
00592                         text += current.toString() + " ";
00593                 }
00594         }
00595         text += "]";
00596         return text;
00597 }
00598 
00599 }       // namespace datatypes


libsick_ldmrs
Author(s): SICK AG , Martin Günther , Jochen Sprickerhof
autogenerated on Wed Jun 14 2017 04:04:50