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
30namespace fawkes {
31
32/** Polygon as a vector of 2D points. */
33typedef 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 */
39inline 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 */
66inline bool
67polygon_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 */
126inline 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.
Eigen::Vector2f polygon_centroid(const Polygon2D &p)
Calculate centroid of polygon.
Definition: polygon.h:127
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