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 <algorithm>
24 #include <Eigen/Geometry>
25 
26 namespace fawkes{
27 #if 0 /* just to make Emacs auto-indent happy */
28 }
29 #endif
30 
31 /** @class NavGraphPolygonConstraint <navgraph/constraints/polygon_constraint.h>
32  * Constraint that blocks nodes within and edges touching a polygon.
33  * @author Tim Niemueller
34  */
35 
36 
37 /** Constructor. */
39 {
40  cur_polygon_handle_ = 0;
41 }
42 
43 
44 
45 /** Constructor.
46  * @param polygon polygon to add immediately
47  */
49 {
50  cur_polygon_handle_ = 0;
51  add_polygon(polygon);
52 }
53 
54 /** Virtual empty destructor. */
56 {
57 }
58 
59 
60 /** Add a polygon to constraint list.
61  * @param polygon Polygon to add to the list
62  * @return handle for the added polygon. The handle can be used to remove
63  * the polygon later.
64  */
67 {
68  PolygonHandle handle = ++cur_polygon_handle_;
69  polygons_[handle] = polygon;
70  return handle;
71 }
72 
73 /** Remove a polygon from the constraint list.
74  * @param handle handle of polygon to remove
75  */
76 void
78 {
79  if (polygons_.find(handle) != polygons_.end()) {
80  polygons_.erase(handle);
81  }
82 }
83 
84 
85 /** Get reference to the map of polygons.
86  * @return map reference of polygons
87  */
90 {
91  return polygons_;
92 }
93 
94 
95 /** Remove all polygons. */
96 void
98 {
99  if (! polygons_.empty()) {
100  polygons_.clear();
101  }
102 }
103 
104 /** Check if given point lies inside the polygon.
105  * The point and polygon are assumed to be in the same X-Y plane.
106  * Code based on http://www.visibone.com/inpoly/inpoly.c.txt
107  * Copyright (c) 1995-1996 Galacticomm, Inc. Freeware source code.
108  * Bob Stein and Craig Yap
109  * Adapted from PCL pcl::isXYPointIn2DXYPolygon()
110  * @param point point to check if it lies within the given polygon
111  * @param polygon polygon to check against
112  * @return true if the point lies inside the polygon, false otherwise
113  */
114 bool
115 NavGraphPolygonConstraint::in_poly(const Point &point, const Polygon &polygon)
116 {
117  bool in_poly = false;
118  float x1, x2, y1, y2;
119 
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;
126  if (xnew > xold) {
127  x1 = xold;
128  x2 = xnew;
129  y1 = yold;
130  y2 = ynew;
131  } else {
132  x1 = xnew;
133  x2 = xold;
134  y1 = ynew;
135  y2 = yold;
136  }
137 
138  if ( (xnew < point.x) == (point.x <= xold) &&
139  (point.y - y1) * (x2 - x1) < (y2 - y1) * (point.x - x1) )
140  {
141  in_poly = !in_poly;
142  }
143  xold = xnew;
144  yold = ynew;
145  }
146 
147  // And a last check for the polygon line formed by the last and the first points
148  float xnew = polygon[0].x;
149  float ynew = polygon[0].y;
150  if (xnew > xold) {
151  x1 = xold;
152  x2 = xnew;
153  y1 = yold;
154  y2 = ynew;
155  } else {
156  x1 = xnew;
157  x2 = xold;
158  y1 = ynew;
159  y2 = yold;
160  }
161 
162  if ( (xnew < point.x) == (point.x <= xold) &&
163  (point.y - y1) * (x2 - x1) < (y2 - y1) * (point.x - x1) )
164  {
165  in_poly = !in_poly;
166  }
167 
168  return (in_poly);
169 }
170 
171 
172 /** Check if a line segments lies on a given polygon.
173  * @param p1 first point of line segment
174  * @param p2 second point of line segment
175  * @param polygon polygon to check against
176  * @return true if the line segment lies on the polygon, false otherwise
177  */
178 bool
179 NavGraphPolygonConstraint::on_poly(const Point &p1, const Point &p2, const Polygon &polygon)
180 {
181  if (polygon.size() < 3) return false;
182 
183  for (size_t i = 0; i < polygon.size() - 1; ++i) {
184  const Point &pol1 = polygon[i ];
185  const Point &pol2 = polygon[i+1];
186 
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);
196 
197 #if EIGEN_VERSION_AT_LEAST(3,2,0)
198  const Eigen::Vector2f is = l1.intersectionPoint(lh);
199 #else
200  const Eigen::Vector2f::Scalar ip = l1.intersection(lh);
201  const Eigen::Vector2f is = l1.origin() + (l1.direction() * ip);
202 #endif
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();
207 
208  if ( t1 >= 0. && t1 <= 1. && t2 >= 0. && t2 <= 1. ) {
209  return true;
210  }
211  }
212 
213  return false;
214 }
215 
216 } // 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.