Fawkes API  Fawkes Development Version
polygon.h
1 
2 /***************************************************************************
3  * polygon.h - polygon related utility methods
4  *
5  * Created: Sat Jul 11 18:34:01 2015
6  * Copyright 2015 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version. A runtime exception applies to
14  * this software (see LICENSE.GPL_WRE file mentioned below for details).
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
22  */
23 
24 #ifndef __UTILS_MATH_POLYGON_H_
25 #define __UTILS_MATH_POLYGON_H_
26 
27 #include <Eigen/Core>
28 #include <Eigen/StdVector>
29 
30 namespace fawkes {
31 #if 0 /* just to make Emacs auto-indent happy */
32 }
33 #endif
34 
35 /** Polygon as a vector of 2D points. */
36 typedef std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > Polygon2D;
37 
38 /** Calculate area of a polygon.
39  * @param p polygon
40  * @return area of polygon
41  */
42 float
44 {
45  float area = 0.;
46 
47  if (p.size() < 3) return 0.;
48 
49  size_t j = p.size() - 1;
50  for (size_t i = 0; i < p.size(); ++i) {
51  area += (p[j][0] + p[i][0]) * (p[j][1] - p[i][1]);
52  j = i;
53  }
54 
55  return fabsf(area)/2.;
56 }
57 
58 
59 /** Check if given point lies inside the polygon.
60  * The point and polygon are assumed to be in the same X-Y plane.
61  * Code based on http://www.visibone.com/inpoly/inpoly.c.txt
62  * Copyright (c) 1995-1996 Galacticomm, Inc. Freeware source code.
63  * Bob Stein and Craig Yap
64  * Adapted from PCL pcl::isXYPointIn2DXYPolygon()
65  * @param polygon polygon to check against
66  * @param point point to check if it lies within the given polygon
67  * @return true if the point lies inside the polygon, false otherwise
68  */
69 bool
70 polygon_contains(const Polygon2D &polygon, const Eigen::Vector2f &point)
71 {
72  bool in_poly = false;
73  float x1, x2, y1, y2;
74 
75  const int nr_poly_points = static_cast<int>(polygon.size());
76  float xold = polygon[nr_poly_points - 1][0];
77  float yold = polygon[nr_poly_points - 1][1];
78  for (int i = 0; i < nr_poly_points; i++) {
79  float xnew = polygon[i][0];
80  float ynew = polygon[i][1];
81  if (xnew > xold) {
82  x1 = xold;
83  x2 = xnew;
84  y1 = yold;
85  y2 = ynew;
86  } else {
87  x1 = xnew;
88  x2 = xold;
89  y1 = ynew;
90  y2 = yold;
91  }
92 
93  if ( (xnew < point[0]) == (point[0] <= xold) &&
94  (point[1] - y1) * (x2 - x1) < (y2 - y1) * (point[0] - x1) )
95  {
96  in_poly = !in_poly;
97  }
98  xold = xnew;
99  yold = ynew;
100  }
101 
102  // And a last check for the polygon line formed by the last and the first points
103  float xnew = polygon[0][0];
104  float ynew = polygon[0][1];
105  if (xnew > xold) {
106  x1 = xold;
107  x2 = xnew;
108  y1 = yold;
109  y2 = ynew;
110  } else {
111  x1 = xnew;
112  x2 = xold;
113  y1 = ynew;
114  y2 = yold;
115  }
116 
117  if ( (xnew < point[0]) == (point[0] <= xold) &&
118  (point[1] - y1) * (x2 - x1) < (y2 - y1) * (point[0] - x1) )
119  {
120  in_poly = !in_poly;
121  }
122 
123  return (in_poly);
124 }
125 
126 
127 /** Calculate centroid of polygon.
128  * Note that the centroid might even lie outside for an irregular polygon.
129  * @param p polygon
130  * @return centroid
131  */
132 Eigen::Vector2f
134 {
135  Eigen::Vector2f centroid(0.,0.);
136 
137  double area = 0.0;
138  double a = 0.;
139 
140  size_t j = p.size() - 1; // The last vertex is the 'previous' one to the first
141 
142  for (size_t i = 0; i < p.size(); ++i) {
143  a = p[j][0]*+p[i][1] - p[i][0]*p[j][1];
144  area += a;
145  centroid[0] += (p[j][0] + p[i][0]) * a;
146  centroid[1] += (p[j][1] + p[i][1]) * a;
147  j = i;
148  }
149 
150  area *= 0.5;
151  centroid[0] /= (6.0 * area);
152  centroid[1] /= (6.0 * area);
153 
154  return centroid;
155 }
156 
157 } // end namespace fawkes
158 
159 #endif
Fawkes library namespace.
float polygon_area(const Polygon2D &p)
Calculate area of a polygon.
Definition: polygon.h:43
std::vector< Eigen::Vector2f, Eigen::aligned_allocator< Eigen::Vector2f > > Polygon2D
Polygon as a vector of 2D points.
Definition: polygon.h:36
bool polygon_contains(const Polygon2D &polygon, const Eigen::Vector2f &point)
Check if given point lies inside the polygon.
Definition: polygon.h:70
Eigen::Vector2f polygon_centroid(const Polygon2D &p)
Calculate centroid of polygon.
Definition: polygon.h:133