Fawkes API  Fawkes Development Version
polygon_constraint.cpp
1 /***************************************************************************
2  * polygon_constraint.cpp -
3  *
4  * Created: Mon Jan 19 11:20:31 2015 (next to Super-C waiting for demo)
5  * Copyright 2015 Tim Niemueller
6  ****************************************************************************/
7 
8 /* This program is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU Library General Public License for more details.
17  *
18  * Read the full text in the LICENSE.GPL file in the doc directory.
19  */
20 
21 #include <navgraph/constraints/polygon_constraint.h>
22 
23 #include <Eigen/Geometry>
24 #include <algorithm>
25 
26 namespace fawkes {
27 
28 /** @class NavGraphPolygonConstraint <navgraph/constraints/polygon_constraint.h>
29  * Constraint that blocks nodes within and edges touching a polygon.
30  * @author Tim Niemueller
31  */
32 
33 /** Constructor. */
35 {
36  cur_polygon_handle_ = 0;
37 }
38 
39 /** Constructor.
40  * @param polygon polygon to add immediately
41  */
43 {
44  cur_polygon_handle_ = 0;
45  add_polygon(polygon);
46 }
47 
48 /** Virtual empty destructor. */
50 {
51 }
52 
53 /** Add a polygon to constraint list.
54  * @param polygon Polygon to add to the list
55  * @return handle for the added polygon. The handle can be used to remove
56  * the polygon later.
57  */
60 {
61  PolygonHandle handle = ++cur_polygon_handle_;
62  polygons_[handle] = polygon;
63  return handle;
64 }
65 
66 /** Remove a polygon from the constraint list.
67  * @param handle handle of polygon to remove
68  */
69 void
71 {
72  if (polygons_.find(handle) != polygons_.end()) {
73  polygons_.erase(handle);
74  }
75 }
76 
77 /** Get reference to the map of polygons.
78  * @return map reference of polygons
79  */
82 {
83  return polygons_;
84 }
85 
86 /** Remove all polygons. */
87 void
89 {
90  if (!polygons_.empty()) {
91  polygons_.clear();
92  }
93 }
94 
95 /** Check if given point lies inside the polygon.
96  * The point and polygon are assumed to be in the same X-Y plane.
97  * Code based on http://www.visibone.com/inpoly/inpoly.c.txt
98  * Copyright (c) 1995-1996 Galacticomm, Inc. Freeware source code.
99  * Bob Stein and Craig Yap
100  * Adapted from PCL pcl::isXYPointIn2DXYPolygon()
101  * @param point point to check if it lies within the given polygon
102  * @param polygon polygon to check against
103  * @return true if the point lies inside the polygon, false otherwise
104  */
105 bool
106 NavGraphPolygonConstraint::in_poly(const Point &point, const Polygon &polygon)
107 {
108  bool in_poly = false;
109  float x1, x2, y1, y2;
110 
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;
117  if (xnew > xold) {
118  x1 = xold;
119  x2 = xnew;
120  y1 = yold;
121  y2 = ynew;
122  } else {
123  x1 = xnew;
124  x2 = xold;
125  y1 = ynew;
126  y2 = yold;
127  }
128 
129  if ((xnew < point.x) == (point.x <= xold)
130  && (point.y - y1) * (x2 - x1) < (y2 - y1) * (point.x - x1)) {
131  in_poly = !in_poly;
132  }
133  xold = xnew;
134  yold = ynew;
135  }
136 
137  // And a last check for the polygon line formed by the last and the first points
138  float xnew = polygon[0].x;
139  float ynew = polygon[0].y;
140  if (xnew > xold) {
141  x1 = xold;
142  x2 = xnew;
143  y1 = yold;
144  y2 = ynew;
145  } else {
146  x1 = xnew;
147  x2 = xold;
148  y1 = ynew;
149  y2 = yold;
150  }
151 
152  if ((xnew < point.x) == (point.x <= xold)
153  && (point.y - y1) * (x2 - x1) < (y2 - y1) * (point.x - x1)) {
154  in_poly = !in_poly;
155  }
156 
157  return (in_poly);
158 }
159 
160 /** Check if a line segments lies on a given polygon.
161  * @param p1 first point of line segment
162  * @param p2 second point of line segment
163  * @param polygon polygon to check against
164  * @return true if the line segment lies on the polygon, false otherwise
165  */
166 bool
167 NavGraphPolygonConstraint::on_poly(const Point &p1, const Point &p2, const Polygon &polygon)
168 {
169  if (polygon.size() < 3)
170  return false;
171 
172  for (size_t i = 0; i < polygon.size() - 1; ++i) {
173  const Point &pol1 = polygon[i];
174  const Point &pol2 = polygon[i + 1];
175 
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);
185 
186 #if EIGEN_VERSION_AT_LEAST(3, 2, 0)
187  const Eigen::Vector2f is = l1.intersectionPoint(lh);
188 #else
189  const Eigen::Vector2f::Scalar ip = l1.intersection(lh);
190  const Eigen::Vector2f is = l1.origin() + (l1.direction() * ip);
191 #endif
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();
196 
197  if (t1 >= 0. && t1 <= 1. && t2 >= 0. && t2 <= 1.) {
198  return true;
199  }
200  }
201 
202  return false;
203 }
204 
205 } // end of namespace fawkes
PolygonMap polygons_
currently registered polygons
PolygonHandle add_polygon(const Polygon &polygon)
Add a polygon to constraint list.
virtual ~NavGraphPolygonConstraint()
Virtual empty destructor.
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.
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.