21 #include <navgraph/constraints/polygon_constraint.h> 24 #include <Eigen/Geometry> 38 NavGraphPolygonConstraint::NavGraphPolygonConstraint()
40 cur_polygon_handle_ = 0;
48 NavGraphPolygonConstraint::NavGraphPolygonConstraint(
const Polygon &polygon)
50 cur_polygon_handle_ = 0;
55 NavGraphPolygonConstraint::~NavGraphPolygonConstraint()
69 polygons_[handle] = polygon;
79 if (polygons_.find(handle) != polygons_.end()) {
80 polygons_.erase(handle);
89 NavGraphPolygonConstraint::polygons()
const 97 NavGraphPolygonConstraint::clear_polygons()
99 if (! polygons_.empty()) {
115 NavGraphPolygonConstraint::in_poly(
const Point &point,
const Polygon &polygon)
117 bool in_poly =
false;
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. ) {
float x
X coordinate of point.
Simple point representation for polygon.
Fawkes library namespace.
std::map< PolygonHandle, Polygon > PolygonMap
Map for accessing all polygons at once with their handles.
float y
Y coordinate of point.
unsigned int PolygonHandle
Handle for polygon for selective removal.
std::vector< Point > Polygon
A vector of points makes a polygon.