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