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());
52 NavGraphEdge::NavGraphEdge()
64 NavGraphEdge::NavGraphEdge(
const std::string &from,
const std::string &to,
65 std::map<std::string, std::string> properties,
70 properties_ = properties;
80 NavGraphEdge::NavGraphEdge(
const std::string &from,
const std::string &to,
bool directed)
91 NavGraphEdge::set_from(
const std::string &from)
101 NavGraphEdge::set_to(
const std::string &to)
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;
132 NavGraphEdge::set_directed(
bool directed)
134 directed_ = directed;
142 NavGraphEdge::property(
const std::string &prop)
const 144 std::map<std::string, std::string>::const_iterator p;
145 if ((p = properties_.find(prop)) != properties_.end()) {
156 NavGraphEdge::set_properties(
const std::map<std::string, std::string> &properties)
158 properties_ = properties;
166 NavGraphEdge::set_property(
const std::string &property,
const std::string &value)
168 properties_[property] = value;
177 NavGraphEdge::set_property(
const std::string &property,
const char *value)
179 properties_[property] = value;
188 NavGraphEdge::set_property(
const std::string &property,
float value)
190 properties_[property] = StringConversions::to_string(value);
198 NavGraphEdge::set_property(
const std::string &property,
int value)
200 properties_[property] = StringConversions::to_string(value);
208 NavGraphEdge::set_property(
const std::string &property,
bool value)
210 properties_[property] = value ?
"true" :
"false";
226 NavGraphEdge::closest_point_on_edge(
float x,
float y)
const 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());
256 NavGraphEdge::intersection(
float x1,
float y1,
float x2,
float y2,
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)) {
285 NavGraphEdge::intersects(
float x1,
float y1,
float x2,
float y2)
const 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);
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).
Fawkes library namespace.
Base class for exceptions in Fawkes.
const std::string & name() const
Get name of node.
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.