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.) {
PolygonMap polygons_
currently registered polygons
PolygonHandle add_polygon(const Polygon &polygon)
Add a polygon to constraint list.
virtual ~NavGraphPolygonConstraint()
Virtual empty destructor.
float x
X coordinate of point.
Simple point representation for polygon.
Fawkes library namespace.
void remove_polygon(const PolygonHandle &handle)
Remove a polygon from the constraint list.
std::map< PolygonHandle, Polygon > PolygonMap
Map for accessing all polygons at once with their handles.
float y
Y coordinate of point.
NavGraphPolygonConstraint()
Constructor.
bool on_poly(const Point &p1, const Point &p2, const Polygon &polygon)
Check if a line segments lies on a given polygon.
unsigned int PolygonHandle
Handle for polygon for selective removal.
std::vector< Point > Polygon
A vector of points makes a polygon.
void clear_polygons()
Remove all polygons.
bool in_poly(const Point &point, const Polygon &polygon)
Check if given point lies inside the polygon.
const PolygonMap & polygons() const
Get reference to the map of polygons.