Fawkes API Fawkes Development Version
navgraph_edge.cpp
1
2/***************************************************************************
3 * navgraph_edge.cpp - Topological graph
4 *
5 * Created: Fri Sep 21 16:11:50 2012
6 * Copyright 2012 Tim Niemueller [www.niemueller.de]
7 ****************************************************************************/
8
9/* This program is free software; you can redistribute it and/or modify
10 * it under the terms of the GNU General Public License as published by
11 * the Free Software Foundation; either version 2 of the License, or
12 * (at your option) any later version. A runtime exception applies to
13 * this software (see LICENSE.GPL_WRE file mentioned below for details).
14 *
15 * This program is distributed in the hope that it will be useful,
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 * GNU Library General Public License for more details.
19 *
20 * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
21 */
22
23#include <core/exception.h>
24#include <navgraph/navgraph_edge.h>
25#include <utils/math/lines.h>
26
27#include <Eigen/Geometry>
28
29namespace fawkes {
30
31#if !EIGEN_VERSION_AT_LEAST(3, 2, 0)
32namespace workaround {
33template <typename Derived>
34static inline bool
35allFinite(const Eigen::DenseBase<Derived> &d)
36{
37 const Derived t = (d.derived() - d.derived());
38 return ((t.derived().array() == t.derived().array()).all());
39}
40} // namespace workaround
41#endif
42
43/** @class NavGraphEdge <navgraph/navgraph_edge.h>
44 * Topological graph edge.
45 * @author Tim Niemueller
46 */
47
48/** Constructor for an invalid edge. */
50{
51 directed_ = false;
52}
53
54/** Constructor.
55 * @param from originating node name
56 * @param to target node name
57 * @param properties properties of the new node
58 * @param directed true if the edge is directed, false for bidirectional edges
59 */
60NavGraphEdge::NavGraphEdge(const std::string & from,
61 const std::string & to,
62 std::map<std::string, std::string> properties,
63 bool directed)
64{
65 from_ = from;
66 to_ = to;
67 properties_ = properties;
68 directed_ = directed;
69}
70
71/** Constructor.
72 * @param from originating node name
73 * @param to target node name
74 * @param directed true if the edge is directed, false for bidirectional edges
75 */
76NavGraphEdge::NavGraphEdge(const std::string &from, const std::string &to, bool directed)
77{
78 from_ = from;
79 to_ = to;
80 directed_ = directed;
81}
82
83/** Set originating node name.
84 * @param from originating node name
85 */
86void
87NavGraphEdge::set_from(const std::string &from)
88{
89 from_ = from;
90}
91
92/** Set target node name.
93 * @param to target node name
94 */
95void
96NavGraphEdge::set_to(const std::string &to)
97{
98 to_ = to;
99}
100
101/** Set nodes.
102 * @param from_node originating node
103 * @param to_node target node
104 */
105void
106NavGraphEdge::set_nodes(const NavGraphNode &from_node, const NavGraphNode &to_node)
107{
108 if (from_node.name() != from_) {
109 throw Exception("Conflicting originating node names: %s vs. %s",
110 from_node.name().c_str(),
111 from_.c_str());
112 }
113 if (to_node.name() != to_) {
114 throw Exception("Conflicting target node names: %s vs. %s",
115 to_node.name().c_str(),
116 to_.c_str());
117 }
118
119 from_node_ = from_node;
120 to_node_ = to_node;
121}
122
123/** Set directed state.
124 * @param directed true if the edge is directed, false for bidirectional edges
125 */
126void
128{
129 directed_ = directed;
130}
131
132/** Get specified property as string.
133 * @param prop property key
134 * @return property value as string
135 */
136std::string
137NavGraphEdge::property(const std::string &prop) const
138{
139 std::map<std::string, std::string>::const_iterator p;
140 if ((p = properties_.find(prop)) != properties_.end()) {
141 return p->second;
142 } else {
143 return "";
144 }
145}
146
147/** Overwrite properties with given ones.
148 * @param properties map of properties to set
149 */
150void
151NavGraphEdge::set_properties(const std::map<std::string, std::string> &properties)
152{
153 properties_ = properties;
154}
155
156/** Set property.
157 * @param property property key
158 * @param value property value
159 */
160void
161NavGraphEdge::set_property(const std::string &property, const std::string &value)
162{
163 properties_[property] = value;
164}
165
166/** Set property.
167 * @param property property key
168 * @param value property value
169 */
170void
171NavGraphEdge::set_property(const std::string &property, const char *value)
172{
173 properties_[property] = value;
174}
175
176/** Set property.
177 * @param property property key
178 * @param value property value
179 */
180void
181NavGraphEdge::set_property(const std::string &property, float value)
182{
183 properties_[property] = StringConversions::to_string(value);
184}
185
186/** Set property.
187 * @param property property key
188 * @param value property value
189 */
190void
191NavGraphEdge::set_property(const std::string &property, int value)
192{
193 properties_[property] = StringConversions::to_string(value);
194}
195
196/** Set property.
197 * @param property property key
198 * @param value property value
199 */
200void
201NavGraphEdge::set_property(const std::string &property, bool value)
202{
203 properties_[property] = value ? "true" : "false";
204}
205
206/** Get the point on edge closest to a given point.
207 * The method determines a line perpendicular to the edge which goes through
208 * the given point, i.e. the point must be within the imaginary line segment.
209 * Then the point on the edge which crosses with that perpendicular line
210 * is returned.
211 * @param x X coordinate of point to get point on edge for
212 * @param y Y coordinate of point to get point on edge for
213 * @return coordinate of point on edge closest to given point
214 * @throw Exception thrown if the point is out of the line segment and
215 * no line perpendicular to the edge going through the given point can
216 * be found.
217 */
220{
221 const Eigen::Vector2f point(x, y);
222 const Eigen::Vector2f origin(from_node_.x(), from_node_.y());
223 const Eigen::Vector2f target(to_node_.x(), to_node_.y());
224 const Eigen::Vector2f direction(target - origin);
225 const Eigen::Vector2f direction_norm = direction.normalized();
226 const Eigen::Vector2f diff = point - origin;
227 const float t = direction.dot(diff) / direction.squaredNorm();
228
229 if (t >= 0.0 && t <= 1.0) {
230 // projection of the point onto the edge is within the line segment
231 Eigen::Vector2f point_on_line = origin + direction_norm.dot(diff) * direction_norm;
232 return cart_coord_2d_t(point_on_line[0], point_on_line[1]);
233 }
234
235 throw Exception("Point (%f,%f) is not on edge %s--%s", x, y, from_.c_str(), to_.c_str());
236}
237
238/** Get distance of point to closest point on edge.
239 * @param x X coordinate of point to get distance to closest point on edge for
240 * @param y Y coordinate of point to get distance to closest point on edge for
241 * @return distance to the closest point on the edge
242 */
243float
244NavGraphEdge::distance(float x, float y) const
245{
247 Eigen::Vector2f p;
248 p[0] = x;
249 p[1] = y;
250 Eigen::Vector2f cp;
251 cp[0] = poe.x;
252 cp[1] = poe.y;
253 return (p - cp).norm();
254}
255
256/** Check if the edge intersects with another line segment.
257 * @param x1 X coordinate of first point of line segment to test
258 * @param y1 Y coordinate of first point of line segment to test
259 * @param x2 X coordinate of first point of line segment to test
260 * @param y2 Y coordinate of first point of line segment to test
261 * @param ip upon returning true contains intersection point,
262 * not modified is return value is false
263 * @return true if the edge intersects with the line segment, false otherwise
264 */
265bool
266NavGraphEdge::intersection(float x1, float y1, float x2, float y2, fawkes::cart_coord_2d_t &ip)
267 const
268{
269 const Eigen::Vector2f e_from(from_node_.x(), from_node_.y());
270 const Eigen::Vector2f e_to(to_node_.x(), to_node_.y());
271 const Eigen::Vector2f p1(x1, y1);
272 const Eigen::Vector2f p2(x2, y2);
273 const Eigen::Vector2f lip = line_segm_intersection(e_from, e_to, p1, p2);
274#if EIGEN_VERSION_AT_LEAST(3, 2, 0)
275 if (lip.allFinite()) {
276#else
277 if (workaround::allFinite(lip)) {
278#endif
279 ip.x = lip[0];
280 ip.y = lip[1];
281 return true;
282 } else {
283 return false;
284 }
285}
286
287/** Check if the edge intersects with another line segment.
288 * @param x1 X coordinate of first point of line segment to test
289 * @param y1 Y coordinate of first point of line segment to test
290 * @param x2 X coordinate of first point of line segment to test
291 * @param y2 Y coordinate of first point of line segment to test
292 * @return true if the edge intersects with the line segment, false otherwise
293 */
294bool
295NavGraphEdge::intersects(float x1, float y1, float x2, float y2) const
296{
297 const Eigen::Vector2f e_from(from_node_.x(), from_node_.y());
298 const Eigen::Vector2f e_to(to_node_.x(), to_node_.y());
299 const Eigen::Vector2f p1(x1, y1);
300 const Eigen::Vector2f p2(x2, y2);
301 return line_segm_intersect(e_from, e_to, p1, p2);
302}
303
304} // end of namespace fawkes
Base class for exceptions in Fawkes.
Definition: exception.h:36
float distance(float x, float y) const
Get distance of point to closest point on edge.
NavGraphEdge()
Constructor for an invalid edge.
void set_property(const std::string &property, const std::string &value)
Set property.
bool intersection(float x1, float y1, float x2, float y2, fawkes::cart_coord_2d_t &ip) const
Check if the edge intersects with another line segment.
const std::string & to() const
Get edge target node name.
Definition: navgraph_edge.h:62
void set_nodes(const NavGraphNode &from_node, const NavGraphNode &to_node)
Set nodes.
fawkes::cart_coord_2d_t closest_point_on_edge(float x, float y) const
Get the point on edge closest to a given point.
void set_from(const std::string &from)
Set originating node name.
const std::map< std::string, std::string > & properties() const
Get all properties.
Definition: navgraph_edge.h:96
const std::string & from() const
Get edge originating node name.
Definition: navgraph_edge.h:54
bool intersects(float x1, float y1, float x2, float y2) const
Check if the edge intersects with another line segment.
void set_properties(const std::map< std::string, std::string > &properties)
Overwrite properties with given ones.
const NavGraphNode & from_node() const
Get edge originating node.
Definition: navgraph_edge.h:70
void set_to(const std::string &to)
Set target node name.
std::string property(const std::string &prop) const
Get specified property as string.
void set_directed(bool directed)
Set directed state.
const NavGraphNode & to_node() const
Get edge target node.
Definition: navgraph_edge.h:78
Topological graph node.
Definition: navgraph_node.h:36
float x() const
Get X coordinate in global frame.
Definition: navgraph_node.h:58
const std::string & name() const
Get name of node.
Definition: navgraph_node.h:50
float y() const
Get Y coordinate in global frame.
Definition: navgraph_node.h:66
static std::string to_string(unsigned int i)
Convert unsigned int value to a string.
Fawkes library namespace.
Eigen::Vector2f line_segm_intersection(const Eigen::Vector2f &l1_from, const Eigen::Vector2f &l1_to, const Eigen::Vector2f &l2_from, const Eigen::Vector2f &l2_to)
Get line segment intersection point.
Definition: lines.h:117
bool line_segm_intersect(const Eigen::Vector2f &l1_from, const Eigen::Vector2f &l1_to, const Eigen::Vector2f &l2_from, const Eigen::Vector2f &l2_to)
Check if two line segments intersect.
Definition: lines.h:40
struct fawkes::cart_coord_2d_struct cart_coord_2d_t
Cartesian coordinates (2D).
Cartesian coordinates (2D).
Definition: types.h:65
float y
y coordinate
Definition: types.h:67
float x
x coordinate
Definition: types.h:66