21 #include <navgraph/constraints/polygon_constraint.h>
23 #include <Eigen/Geometry>
36 cur_polygon_handle_ = 0;
44 cur_polygon_handle_ = 0;
109 float x1, x2, y1, y2;
111 const int nr_poly_points = static_cast<int>(polygon.size());
112 float xold = polygon[nr_poly_points - 1].x;
113 float yold = polygon[nr_poly_points - 1].y;
114 for (
int i = 0; i < nr_poly_points; i++) {
115 float xnew = polygon[i].x;
116 float ynew = polygon[i].y;
129 if ((xnew < point.x) == (point.x <= xold)
130 && (point.y - y1) * (x2 - x1) < (y2 - y1) * (point.x - x1)) {
138 float xnew = polygon[0].x;
139 float ynew = polygon[0].y;
152 if ((xnew < point.x) == (point.x <= xold)
153 && (point.y - y1) * (x2 - x1) < (y2 - y1) * (point.x - x1)) {
169 if (polygon.size() < 3)
172 for (
size_t i = 0; i < polygon.size() - 1; ++i) {
173 const Point &pol1 = polygon[i];
174 const Point &pol2 = polygon[i + 1];
176 const Eigen::Vector2f pp1(p1.x, p1.y);
177 const Eigen::Vector2f pp2(p2.x, p2.y);
178 const Eigen::Vector2f ep1(pol1.
x, pol1.
y);
179 const Eigen::Vector2f ep2(pol2.
x, pol2.
y);
180 const Eigen::ParametrizedLine<float, 2> l1 =
181 Eigen::ParametrizedLine<float, 2>::Through(pp1, pp2);
182 const Eigen::ParametrizedLine<float, 2> l2 =
183 Eigen::ParametrizedLine<float, 2>::Through(ep1, ep2);
184 const Eigen::Hyperplane<float, 2> lh(l2);
186 #if EIGEN_VERSION_AT_LEAST(3, 2, 0)
187 const Eigen::Vector2f is = l1.intersectionPoint(lh);
189 const Eigen::Vector2f::Scalar ip = l1.intersection(lh);
190 const Eigen::Vector2f is = l1.origin() + (l1.direction() * ip);
192 const Eigen::Vector2f d1 = pp2 - pp1;
193 const Eigen::Vector2f d2 = ep2 - ep1;
194 const float t1 = d1.dot(is - l1.origin()) / d1.squaredNorm();
195 const float t2 = d2.dot(is - l2.origin()) / d2.squaredNorm();
197 if (t1 >= 0. && t1 <= 1. && t2 >= 0. && t2 <= 1.) {