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 
32 /** Polygon as a vector of 2D points. */
33 typedef std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f>> Polygon2D;
34 
35 /** Calculate area of a polygon.
36  * @param p polygon
37  * @return area of polygon
38  */
39 inline float
41 {
42  float area = 0.;
43 
44  if (p.size() < 3)
45  return 0.;
46 
47  size_t j = p.size() - 1;
48  for (size_t i = 0; i < p.size(); ++i) {
49  area += (p[j][0] + p[i][0]) * (p[j][1] - p[i][1]);
50  j = i;
51  }
52 
53  return fabsf(area) / 2.;
54 }
55 
56 /** Check if given point lies inside the polygon.
57  * The point and polygon are assumed to be in the same X-Y plane.
58  * Code based on http://www.visibone.com/inpoly/inpoly.c.txt
59  * Copyright (c) 1995-1996 Galacticomm, Inc. Freeware source code.
60  * Bob Stein and Craig Yap
61  * Adapted from PCL pcl::isXYPointIn2DXYPolygon()
62  * @param polygon polygon to check against
63  * @param point point to check if it lies within the given polygon
64  * @return true if the point lies inside the polygon, false otherwise
65  */
66 inline bool
67 polygon_contains(const Polygon2D &polygon, const Eigen::Vector2f &point)
68 {
69  bool in_poly = false;
70  float x1, x2, y1, y2;
71 
72  const int nr_poly_points = static_cast<int>(polygon.size());
73  float xold = polygon[nr_poly_points - 1][0];
74  float yold = polygon[nr_poly_points - 1][1];
75  for (int i = 0; i < nr_poly_points; i++) {
76  float xnew = polygon[i][0];
77  float ynew = polygon[i][1];
78  if (xnew > xold) {
79  x1 = xold;
80  x2 = xnew;
81  y1 = yold;
82  y2 = ynew;
83  } else {
84  x1 = xnew;
85  x2 = xold;
86  y1 = ynew;
87  y2 = yold;
88  }
89 
90  if ((xnew < point[0]) == (point[0] <= xold)
91  && (point[1] - y1) * (x2 - x1) < (y2 - y1) * (point[0] - x1)) {
92  in_poly = !in_poly;
93  }
94  xold = xnew;
95  yold = ynew;
96  }
97 
98  // And a last check for the polygon line formed by the last and the first points
99  float xnew = polygon[0][0];
100  float ynew = polygon[0][1];
101  if (xnew > xold) {
102  x1 = xold;
103  x2 = xnew;
104  y1 = yold;
105  y2 = ynew;
106  } else {
107  x1 = xnew;
108  x2 = xold;
109  y1 = ynew;
110  y2 = yold;
111  }
112 
113  if ((xnew < point[0]) == (point[0] <= xold)
114  && (point[1] - y1) * (x2 - x1) < (y2 - y1) * (point[0] - x1)) {
115  in_poly = !in_poly;
116  }
117 
118  return (in_poly);
119 }
120 
121 /** Calculate centroid of polygon.
122  * Note that the centroid might even lie outside for an irregular polygon.
123  * @param p polygon
124  * @return centroid
125  */
126 inline Eigen::Vector2f
128 {
129  Eigen::Vector2f centroid(0., 0.);
130 
131  double area = 0.0;
132  double a = 0.;
133 
134  size_t j = p.size() - 1; // The last vertex is the 'previous' one to the first
135 
136  for (size_t i = 0; i < p.size(); ++i) {
137  a = p[j][0] * +p[i][1] - p[i][0] * p[j][1];
138  area += a;
139  centroid[0] += (p[j][0] + p[i][0]) * a;
140  centroid[1] += (p[j][1] + p[i][1]) * a;
141  j = i;
142  }
143 
144  area *= 0.5;
145  centroid[0] /= (6.0 * area);
146  centroid[1] /= (6.0 * area);
147 
148  return centroid;
149 }
150 
151 } // end namespace fawkes
152 
153 #endif
Fawkes library namespace.
float polygon_area(const Polygon2D &p)
Calculate area of a polygon.
Definition: polygon.h:40
std::vector< Eigen::Vector2f, Eigen::aligned_allocator< Eigen::Vector2f > > Polygon2D
Polygon as a vector of 2D points.
Definition: polygon.h:33
bool polygon_contains(const Polygon2D &polygon, const Eigen::Vector2f &point)
Check if given point lies inside the polygon.
Definition: polygon.h:67
Eigen::Vector2f polygon_centroid(const Polygon2D &p)
Calculate centroid of polygon.
Definition: polygon.h:127