23 #include <navgraph/navgraph_edge.h> 25 #include <core/exception.h> 26 #include <utils/math/lines.h> 28 #include <Eigen/Geometry> 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)
40 const Derived t = (d.derived()-d.derived());
41 return ((t.derived().array()==t.derived().array()).all());
65 std::map<std::string, std::string> properties,
70 properties_ = properties;
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());
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());
124 from_node_ = from_node;
134 directed_ = directed;
144 std::map<std::string, std::string>::const_iterator p;
145 if ((p = properties_.find(prop)) != properties_.end()) {
158 properties_ = properties;
168 properties_[property] = value;
179 properties_[property] = value;
210 properties_[property] = value ?
"true" :
"false";
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();
236 if (t >= 0.0 && t <= 1.0) {
238 Eigen::Vector2f point_on_line = origin + direction_norm.dot(diff) * direction_norm;
242 throw Exception(
"Point (%f,%f) is not on edge %s--%s", x, y,
243 from_.c_str(), to_.c_str());
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);
264 #if EIGEN_VERSION_AT_LEAST(3,2,0) 265 if (lip.allFinite()) {
267 if (workaround::allFinite(lip)) {
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);
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.
Cartesian coordinates (2D).
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.
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.
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.
static std::string to_string(unsigned int i)
Convert unsigned int value to a string.