42 #ifndef OMPL_DATASTRUCTURES_DYNAMICSSSP_H
43 #define OMPL_DATASTRUCTURES_DYNAMICSSSP_H
50 #include <boost/functional/hash.hpp>
51 #include <boost/graph/graph_traits.hpp>
52 #include <boost/graph/adjacency_list.hpp>
53 #include <unordered_set>
62 graph_ =
new Graph(0);
76 void addVertex(std::size_t
id)
78 distance_.push_back((
id == 0) ? 0 : std::numeric_limits<double>::infinity());
79 parent_.push_back(NO_ID);
80 boost::add_vertex(
id, *graph_);
85 void addEdge(std::size_t v, std::size_t w,
double weight,
bool collectVertices,
86 std::list<std::size_t> &affectedVertices)
89 WeightProperty edge_property(weight);
90 boost::add_edge(v, w, edge_property, *graph_);
93 assert((distance_[v] == std::numeric_limits<double>::infinity()) ||
94 (distance_[w] == std::numeric_limits<double>::infinity()) ||
95 (distance_[w] + weight != distance_[w]));
97 std::vector<double> cost(boost::num_vertices(*graph_),
98 std::numeric_limits<double>::infinity());
100 IsLessThan isLessThan(cost);
101 Queue queue(isLessThan);
103 if (distance_[v] + weight < distance_[w])
105 distance_[w] = distance_[v] + weight;
112 WeightMap weights = boost::get(boost::edge_weight_t(), *graph_);
113 while (!queue.empty())
116 std::size_t u = *(queue.begin());
117 queue.erase(queue.begin());
120 affectedVertices.push_back(u);
122 boost::out_edges(u, *graph_);
125 boost::graph_traits<Graph>::out_edge_iterator ei, ei_end;
126 for (boost::tie(ei, ei_end) = boost::out_edges(u, *graph_); ei != ei_end; ++ei)
128 std::size_t x = boost::target(*ei, *graph_);
129 double edgeWeight = boost::get(weights, *ei);
131 if (distance_[u] + edgeWeight < distance_[x])
133 distance_[x] = distance_[u] + edgeWeight;
137 auto qIter = queue.find(x);
138 if (qIter != queue.end())
141 cost[x] = distance_[x] - distance_[v];
148 void removeEdge(std::size_t v, std::size_t w,
bool collectVertices, std::list<std::size_t> &affectedVertices)
151 boost::remove_edge(v, w, *graph_);
156 std::list<std::size_t> workSet;
157 IntSet affectedVerticesSet;
158 workSet.push_back(w);
160 while (!workSet.empty())
163 std::size_t u = workSet.front();
166 affectedVerticesSet.insert(u);
168 boost::graph_traits<Graph>::out_edge_iterator ei, ei_end;
169 for (boost::tie(ei, ei_end) = boost::out_edges(u, *graph_); ei != ei_end; ++ei)
171 std::size_t x = boost::target(*ei, *graph_);
173 workSet.push_back(x);
177 WeightMap weights = boost::get(boost::edge_weight_t(), *graph_);
180 IsLessThan isLessThan(distance_);
181 Queue queue(isLessThan);
182 for (
auto set_iter = affectedVerticesSet.begin(); set_iter != affectedVerticesSet.end(); ++set_iter)
184 std::size_t a = *set_iter;
185 distance_[a] = std::numeric_limits<double>::infinity();
189 boost::graph_traits<Graph>::in_edge_iterator ei, ei_end;
190 for (boost::tie(ei, ei_end) = boost::in_edges(a, *graph_); ei != ei_end; ++ei)
192 std::size_t b = boost::source(*ei, *graph_);
193 if (affectedVerticesSet.find(b) == affectedVerticesSet.end())
195 double edgeWeight = boost::get(weights, *ei);
197 if (distance_[b] + edgeWeight < distance_[a])
199 distance_[a] = distance_[b] + edgeWeight;
204 if (distance_[a] != std::numeric_limits<double>::infinity())
208 while (!queue.empty())
211 std::size_t a = *queue.begin();
212 queue.erase(queue.begin());
215 affectedVertices.push_back(a);
218 boost::graph_traits<Graph>::out_edge_iterator ei, ei_end;
219 for (boost::tie(ei, ei_end) = boost::out_edges(a, *graph_); ei != ei_end; ++ei)
221 int c = boost::target(*ei, *graph_);
222 double edgeWeight = boost::get(weights, *ei);
224 if (distance_[a] + edgeWeight < distance_[c])
226 distance_[c] = distance_[a] + edgeWeight;
230 auto qIter = queue.find(c);
231 if (qIter != queue.end())
240 double getShortestPathCost(std::size_t u)
const
242 return this->distance_[u];
245 std::size_t getShortestPathParent(std::size_t u)
const
251 using WeightProperty = boost::property<boost::edge_weight_t, double>;
252 using Graph = boost::adjacency_list<boost::vecS,
254 boost::bidirectionalS,
257 using WeightMap = boost::property_map<Graph, boost::edge_weight_t>::type;
259 static const int NO_ID = -1;
264 IsLessThan(std::vector<double> &cost) : cost_(cost)
268 bool operator()(std::size_t id1, std::size_t id2)
const
270 return (cost_[id1] < cost_[id2]);
274 std::vector<double> &cost_;
277 using Queue = std::set<std::size_t, IsLessThan>;
278 using QueueIter = Queue::iterator;
279 using IntSet = std::unordered_set<std::size_t>;
280 using IntSetIter = IntSet::iterator;
284 std::vector<double> distance_;
286 std::vector<std::size_t> parent_;
290 #endif // OMPL_DATASTRUCTURES_DYNAMICSSSP_H