diff --git a/include/hpp/manipulation/graph/node.hh b/include/hpp/manipulation/graph/node.hh index b5c6c590c25d173f8d57db8c277adcd9e0ae9026..9c6d2b1840514a3800100749c62ff81778fc4092 100644 --- a/include/hpp/manipulation/graph/node.hh +++ b/include/hpp/manipulation/graph/node.hh @@ -100,6 +100,8 @@ namespace hpp { void updateWeight (const EdgePtr_t&edge, const Weight_t& w); + Weight_t getWeight (const EdgePtr_t&edge); + /// Constraint to project onto this node. ConstraintSetPtr_t configConstraint() const; diff --git a/src/graph/node.cc b/src/graph/node.cc index dfbabe643e23e21ab18ac79e600e9416b6517e55..b8fbb2d0fe2c0940882effb5dd41e8fe5fd58860 100644 --- a/src/graph/node.cc +++ b/src/graph/node.cc @@ -137,6 +137,18 @@ namespace hpp { } hppDout (error, "Edge not found"); } + + Weight_t Node::getWeight (const EdgePtr_t& e) + { + for (Neighbors_t::const_iterator it = neighbors_.begin(); + it != neighbors_.end(); ++it) + if (it->second == e) return it->first; + for (std::vector<EdgePtr_t>::const_iterator it = hiddenNeighbors_.begin(); + it != hiddenNeighbors_.end(); ++it) + if (*it == e) return -1; + hppDout (error, "Edge not found"); + return 0; + } } // namespace graph } // namespace manipulation } // namespace hpp