23 #include <core/exception.h>
24 #include <navgraph/navgraph_edge.h>
25 #include <utils/math/lines.h>
27 #include <Eigen/Geometry>
31 #if !EIGEN_VERSION_AT_LEAST(3, 2, 0)
32 namespace workaround {
33 template <
typename Derived>
35 allFinite(
const Eigen::DenseBase<Derived> &d)
37 const Derived t = (d.derived() - d.derived());
38 return ((t.derived().array() == t.derived().array()).all());
61 const std::string & to,
62 std::map<std::string, std::string> properties,
109 throw Exception(
"Conflicting originating node names: %s vs. %s",
114 throw Exception(
"Conflicting target node names: %s vs. %s",
129 directed_ = directed;
139 std::map<std::string, std::string>::const_iterator p;
140 if ((p = properties_.find(prop)) != properties_.end()) {
203 properties_[
property] = value ?
"true" :
"false";
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();
229 if (t >= 0.0 && t <= 1.0) {
231 Eigen::Vector2f point_on_line = origin + direction_norm.dot(diff) * direction_norm;
235 throw Exception(
"Point (%f,%f) is not on edge %s--%s", x, y, from_.c_str(), to_.c_str());
253 return (p - cp).norm();
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);
274 #if EIGEN_VERSION_AT_LEAST(3, 2, 0)
275 if (lip.allFinite()) {
277 if (workaround::allFinite(lip)) {
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);
Base class for exceptions in Fawkes.
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::map< std::string, std::string > & properties() const
Get all properties.
void set_nodes(const NavGraphNode &from_node, const NavGraphNode &to_node)
Set nodes.
const NavGraphNode & to_node() const
Get edge target node.
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.
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.
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 std::string & to() const
Get edge target node name.
const std::string & from() const
Get edge originating node name.
float x() const
Get X coordinate in global frame.
const std::string & name() const
Get name of node.
float y() const
Get Y coordinate in global frame.
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.
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.
struct fawkes::cart_coord_2d_struct cart_coord_2d_t
Cartesian coordinates (2D).
Cartesian coordinates (2D).