24 #ifndef _UTILS_MATH_POLYGON_H_
25 #define _UTILS_MATH_POLYGON_H_
28 #include <Eigen/StdVector>
33 typedef std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f>>
Polygon2D;
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]);
53 return fabsf(area) / 2.;
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];
90 if ((xnew < point[0]) == (point[0] <= xold)
91 && (point[1] - y1) * (x2 - x1) < (y2 - y1) * (point[0] - x1)) {
99 float xnew = polygon[0][0];
100 float ynew = polygon[0][1];
113 if ((xnew < point[0]) == (point[0] <= xold)
114 && (point[1] - y1) * (x2 - x1) < (y2 - y1) * (point[0] - x1)) {
126 inline Eigen::Vector2f
129 Eigen::Vector2f centroid(0., 0.);
134 size_t j = p.size() - 1;
136 for (
size_t i = 0; i < p.size(); ++i) {
137 a = p[j][0] * +p[i][1] - p[i][0] * p[j][1];
139 centroid[0] += (p[j][0] + p[i][0]) * a;
140 centroid[1] += (p[j][1] + p[i][1]) * a;
145 centroid[0] /= (6.0 * area);
146 centroid[1] /= (6.0 * area);