21 #include <navgraph/constraints/polygon_constraint.h> 24 #include <Eigen/Geometry> 40 cur_polygon_handle_ = 0;
50 cur_polygon_handle_ = 0;
118 float x1, x2, y1, y2;
120 const int nr_poly_points =
static_cast<int>(polygon.size());
121 float xold = polygon[nr_poly_points - 1].x;
122 float yold = polygon[nr_poly_points - 1].y;
123 for (
int i = 0; i < nr_poly_points; i++) {
124 float xnew = polygon[i].x;
125 float ynew = polygon[i].y;
138 if ( (xnew < point.
x) == (point.
x <= xold) &&
139 (point.
y - y1) * (x2 - x1) < (y2 - y1) * (point.
x - x1) )
148 float xnew = polygon[0].x;
149 float ynew = polygon[0].y;
162 if ( (xnew < point.
x) == (point.
x <= xold) &&
163 (point.
y - y1) * (x2 - x1) < (y2 - y1) * (point.
x - x1) )
181 if (polygon.size() < 3)
return false;
183 for (
size_t i = 0; i < polygon.size() - 1; ++i) {
184 const Point &pol1 = polygon[i ];
185 const Point &pol2 = polygon[i+1];
187 const Eigen::Vector2f pp1(p1.
x, p1.
y);
188 const Eigen::Vector2f pp2(p2.
x, p2.
y);
189 const Eigen::Vector2f ep1(pol1.
x, pol1.
y);
190 const Eigen::Vector2f ep2(pol2.
x, pol2.
y);
191 const Eigen::ParametrizedLine<float,2> l1 =
192 Eigen::ParametrizedLine<float,2>::Through(pp1, pp2);
193 const Eigen::ParametrizedLine<float,2> l2 =
194 Eigen::ParametrizedLine<float,2>::Through(ep1, ep2);
195 const Eigen::Hyperplane<float, 2> lh(l2);
197 #if EIGEN_VERSION_AT_LEAST(3,2,0) 198 const Eigen::Vector2f is = l1.intersectionPoint(lh);
200 const Eigen::Vector2f::Scalar ip = l1.intersection(lh);
201 const Eigen::Vector2f is = l1.origin() + (l1.direction() * ip);
203 const Eigen::Vector2f d1 = pp2 - pp1;
204 const Eigen::Vector2f d2 = ep2 - ep1;
205 const float t1 = d1.dot(is - l1.origin()) / d1.squaredNorm();
206 const float t2 = d2.dot(is - l2.origin()) / d2.squaredNorm();
208 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.