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 
29 namespace fawkes {
30 
31 #if !EIGEN_VERSION_AT_LEAST(3, 2, 0)
32 namespace workaround {
33 template <typename Derived>
34 static inline bool
35 allFinite(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  */
60 NavGraphEdge::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  */
76 NavGraphEdge::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  */
86 void
87 NavGraphEdge::set_from(const std::string &from)
88 {
89  from_ = from;
90 }
91 
92 /** Set target node name.
93  * @param to target node name
94  */
95 void
96 NavGraphEdge::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  */
105 void
106 NavGraphEdge::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  */
126 void
128 {
129  directed_ = directed;
130 }
131 
132 /** Get specified property as string.
133  * @param prop property key
134  * @return property value as string
135  */
136 std::string
137 NavGraphEdge::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  */
150 void
151 NavGraphEdge::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  */
160 void
161 NavGraphEdge::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  */
170 void
171 NavGraphEdge::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  */
180 void
181 NavGraphEdge::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  */
190 void
191 NavGraphEdge::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  */
200 void
201 NavGraphEdge::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  */
219 NavGraphEdge::closest_point_on_edge(float x, float y) const
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  */
243 float
244 NavGraphEdge::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  */
265 bool
266 NavGraphEdge::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  */
294 bool
295 NavGraphEdge::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
const std::string & from() const
Get edge originating node name.
Definition: navgraph_edge.h:54
void set_property(const std::string &property, const std::string &value)
Set property.
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
Cartesian coordinates (2D).
Definition: types.h:64
void set_nodes(const NavGraphNode &from_node, const NavGraphNode &to_node)
Set nodes.
Fawkes library namespace.
float distance(float x, float y) const
Get distance of point to closest point on edge.
void set_properties(const std::map< std::string, std::string > &properties)
Overwrite properties with given ones.
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.
std::string property(const std::string &prop) const
Get specified property as string.
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.
const std::map< std::string, std::string > & properties() const
Get all properties.
Definition: navgraph_edge.h:96
void set_from(const std::string &from)
Set originating node name.
NavGraphEdge()
Constructor for an invalid edge.
fawkes::cart_coord_2d_t closest_point_on_edge(float x, float y) const
Get the point on edge closest to a given point.
Base class for exceptions in Fawkes.
Definition: exception.h:35
const std::string & to() const
Get edge target node name.
Definition: navgraph_edge.h:62
bool intersects(float x1, float y1, float x2, float y2) const
Check if the edge intersects with another line segment.
const std::string & name() const
Get name of node.
Definition: navgraph_node.h:50
void set_directed(bool directed)
Set directed state.
struct fawkes::cart_coord_2d_struct cart_coord_2d_t
Cartesian coordinates (2D).
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
float y() const
Get Y coordinate in global frame.
Definition: navgraph_node.h:66
float y
y coordinate
Definition: types.h:67
Topological graph node.
Definition: navgraph_node.h:35
float x() const
Get X coordinate in global frame.
Definition: navgraph_node.h:58
static std::string to_string(unsigned int i)
Convert unsigned int value to a string.
const NavGraphNode & to_node() const
Get edge target node.
Definition: navgraph_edge.h:78
float x
x coordinate
Definition: types.h:66