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 <navgraph/navgraph_edge.h>
24 
25 #include <core/exception.h>
26 #include <utils/math/lines.h>
27 
28 #include <Eigen/Geometry>
29 
30 namespace fawkes {
31 #if 0 /* just to make Emacs auto-indent happy */
32 }
33 #endif
34 
35 #if ! EIGEN_VERSION_AT_LEAST(3,2,0)
36 namespace workaround {
37  template<typename Derived>
38  static inline bool allFinite(const Eigen::DenseBase<Derived> &d)
39  {
40  const Derived t = (d.derived()-d.derived());
41  return ((t.derived().array()==t.derived().array()).all());
42  }
43 }
44 #endif
45 
46 /** @class NavGraphEdge <navgraph/navgraph_edge.h>
47  * Topological graph edge.
48  * @author Tim Niemueller
49  */
50 
51 /** Constructor for an invalid edge. */
53 {
54  directed_ = false;
55 }
56 
57 
58 /** Constructor.
59  * @param from originating node name
60  * @param to target node name
61  * @param properties properties of the new node
62  * @param directed true if the edge is directed, false for bidirectional edges
63  */
64 NavGraphEdge::NavGraphEdge(const std::string &from, const std::string &to,
65  std::map<std::string, std::string> properties,
66  bool directed)
67 {
68  from_ = from;
69  to_ = to;
70  properties_ = properties;
71  directed_ = directed;
72 }
73 
74 
75 /** Constructor.
76  * @param from originating node name
77  * @param to target node name
78  * @param directed true if the edge is directed, false for bidirectional edges
79  */
80 NavGraphEdge::NavGraphEdge(const std::string &from, const std::string &to, bool directed)
81 {
82  from_ = from;
83  to_ = to;
84  directed_ = directed;
85 }
86 
87 /** Set originating node name.
88  * @param from originating node name
89  */
90 void
91 NavGraphEdge::set_from(const std::string &from)
92 {
93  from_ = from;
94 }
95 
96 
97 /** Set target node name.
98  * @param to target node name
99  */
100 void
101 NavGraphEdge::set_to(const std::string &to)
102 {
103  to_ = to;
104 }
105 
106 
107 /** Set nodes.
108  * @param from_node originating node
109  * @param to_node target node
110  */
111 void
113  const NavGraphNode &to_node)
114 {
115  if (from_node.name() != from_) {
116  throw Exception("Conflicting originating node names: %s vs. %s",
117  from_node.name().c_str(), from_.c_str());
118  }
119  if (to_node.name() != to_) {
120  throw Exception("Conflicting target node names: %s vs. %s",
121  to_node.name().c_str(), to_.c_str());
122  }
123 
124  from_node_ = from_node;
125  to_node_ = to_node;
126 }
127 
128 /** Set directed state.
129  * @param directed true if the edge is directed, false for bidirectional edges
130  */
131 void
133 {
134  directed_ = directed;
135 }
136 
137 /** Get specified property as string.
138  * @param prop property key
139  * @return property value as string
140  */
141 std::string
142 NavGraphEdge::property(const std::string &prop) const
143 {
144  std::map<std::string, std::string>::const_iterator p;
145  if ((p = properties_.find(prop)) != properties_.end()) {
146  return p->second;
147  } else {
148  return "";
149  }
150 }
151 
152 /** Overwrite properties with given ones.
153  * @param properties map of properties to set
154  */
155 void
156 NavGraphEdge::set_properties(const std::map<std::string, std::string> &properties)
157 {
158  properties_ = properties;
159 }
160 
161 /** Set property.
162  * @param property property key
163  * @param value property value
164  */
165 void
166 NavGraphEdge::set_property(const std::string &property, const std::string &value)
167 {
168  properties_[property] = value;
169 }
170 
171 
172 /** Set property.
173  * @param property property key
174  * @param value property value
175  */
176 void
177 NavGraphEdge::set_property(const std::string &property, const char *value)
178 {
179  properties_[property] = value;
180 }
181 
182 
183 /** Set property.
184  * @param property property key
185  * @param value property value
186  */
187 void
188 NavGraphEdge::set_property(const std::string &property, float value)
189 {
190  properties_[property] = StringConversions::to_string(value);
191 }
192 
193 /** Set property.
194  * @param property property key
195  * @param value property value
196  */
197 void
198 NavGraphEdge::set_property(const std::string &property, int value)
199 {
200  properties_[property] = StringConversions::to_string(value);
201 }
202 
203 /** Set property.
204  * @param property property key
205  * @param value property value
206  */
207 void
208 NavGraphEdge::set_property(const std::string &property, bool value)
209 {
210  properties_[property] = value ? "true" : "false";
211 }
212 
213 /** Get the point on edge closest to a given point.
214  * The method determines a line perpendicular to the edge which goes through
215  * the given point, i.e. the point must be within the imaginary line segment.
216  * Then the point on the edge which crosses with that perpendicular line
217  * is returned.
218  * @param x X coordinate of point to get point on edge for
219  * @param y Y coordinate of point to get point on edge for
220  * @return coordinate of point on edge closest to given point
221  * @throw Exception thrown if the point is out of the line segment and
222  * no line perpendicular to the edge going through the given point can
223  * be found.
224  */
226 NavGraphEdge::closest_point_on_edge(float x, float y) const
227 {
228  const Eigen::Vector2f point(x, y);
229  const Eigen::Vector2f origin(from_node_.x(), from_node_.y());
230  const Eigen::Vector2f target(to_node_.x(), to_node_.y());
231  const Eigen::Vector2f direction(target - origin);
232  const Eigen::Vector2f direction_norm = direction.normalized();
233  const Eigen::Vector2f diff = point - origin;
234  const float t = direction.dot(diff) / direction.squaredNorm();
235 
236  if (t >= 0.0 && t <= 1.0) {
237  // projection of the point onto the edge is within the line segment
238  Eigen::Vector2f point_on_line = origin + direction_norm.dot(diff) * direction_norm;
239  return cart_coord_2d_t(point_on_line[0], point_on_line[1]);
240  }
241 
242  throw Exception("Point (%f,%f) is not on edge %s--%s", x, y,
243  from_.c_str(), to_.c_str());
244 }
245 
246 /** Check if the edge intersects with another line segment.
247  * @param x1 X coordinate of first point of line segment to test
248  * @param y1 Y coordinate of first point of line segment to test
249  * @param x2 X coordinate of first point of line segment to test
250  * @param y2 Y coordinate of first point of line segment to test
251  * @param ip upon returning true contains intersection point,
252  * not modified is return value is false
253  * @return true if the edge intersects with the line segment, false otherwise
254  */
255 bool
256 NavGraphEdge::intersection(float x1, float y1, float x2, float y2,
257  fawkes::cart_coord_2d_t &ip) const
258 {
259  const Eigen::Vector2f e_from(from_node_.x(), from_node_.y());
260  const Eigen::Vector2f e_to(to_node_.x(), to_node_.y());
261  const Eigen::Vector2f p1(x1, y1);
262  const Eigen::Vector2f p2(x2, y2);
263  const Eigen::Vector2f lip = line_segm_intersection(e_from, e_to, p1, p2);
264 #if EIGEN_VERSION_AT_LEAST(3,2,0)
265  if (lip.allFinite()) {
266 #else
267  if (workaround::allFinite(lip)) {
268 #endif
269  ip.x = lip[0];
270  ip.y = lip[1];
271  return true;
272  } else {
273  return false;
274  }
275 }
276 
277 /** Check if the edge intersects with another line segment.
278  * @param x1 X coordinate of first point of line segment to test
279  * @param y1 Y coordinate of first point of line segment to test
280  * @param x2 X coordinate of first point of line segment to test
281  * @param y2 Y coordinate of first point of line segment to test
282  * @return true if the edge intersects with the line segment, false otherwise
283  */
284 bool
285 NavGraphEdge::intersects(float x1, float y1, float x2, float y2) const
286 {
287  const Eigen::Vector2f e_from(from_node_.x(), from_node_.y());
288  const Eigen::Vector2f e_to(to_node_.x(), to_node_.y());
289  const Eigen::Vector2f p1(x1, y1);
290  const Eigen::Vector2f p2(x2, y2);
291  return line_segm_intersect(e_from, e_to, p1, p2);
292 }
293 
294 } // end of namespace fawkes
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:114
Cartesian coordinates (2D).
Definition: types.h:59
void set_nodes(const NavGraphNode &from_node, const NavGraphNode &to_node)
Set nodes.
Fawkes library namespace.
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.
void set_to(const std::string &to)
Set target node name.
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:36
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:49
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:43
float y
y coordinate
Definition: types.h:61
Topological graph node.
Definition: navgraph_node.h:38
static std::string to_string(unsigned int i)
Convert unsigned int value to a string.
float x
x coordinate
Definition: types.h:60