Loading...
Searching...
No Matches
DynamicSSSP.h
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2015, Tel Aviv University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Tel Aviv University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34
35/* Author: Oren Salzman, Aditya Mandalika */
36/* Implementation based on
37G. Ramalingam and T. W. Reps, On the computational complexity of
38dynamic graph problems, Theor. Comput. Sci., vol. 158, no. 1&2, pp.
39233-277, 1996.
40*/
41
42#ifndef OMPL_DATASTRUCTURES_DYNAMICSSSP_H
43#define OMPL_DATASTRUCTURES_DYNAMICSSSP_H
44
45#include <list>
46#include <set>
47#include <vector>
48#include <limits>
49
50#include <boost/functional/hash.hpp> // fix for Boost < 1.68
51#include <boost/graph/graph_traits.hpp>
52#include <boost/graph/adjacency_list.hpp>
53#include <unordered_set>
54
55namespace ompl
56{
57 class DynamicSSSP
58 {
59 public:
60 DynamicSSSP()
61 {
62 graph_ = new Graph(0);
63 }
64 ~DynamicSSSP()
65 {
66 delete graph_;
67 }
68
69 void clear()
70 {
71 graph_->clear();
72 distance_.clear();
73 parent_.clear();
74 }
75
76 void addVertex(std::size_t id)
77 {
78 distance_.push_back((id == 0) ? 0 : std::numeric_limits<double>::infinity());
79 parent_.push_back(NO_ID);
80 boost::add_vertex(id, *graph_);
81 }
82
83 // we assume that no two paths have the same cost,
84 // this asssumption is valid when the nodes have some randomeness to them
85 void addEdge(std::size_t v, std::size_t w, double weight, bool collectVertices,
86 std::list<std::size_t> &affectedVertices)
87 {
88 // first, add edge to graph
89 WeightProperty edge_property(weight);
90 boost::add_edge(v, w, edge_property, *graph_);
91
92 // now, update distance_
93 assert((distance_[v] == std::numeric_limits<double>::infinity()) ||
94 (distance_[w] == std::numeric_limits<double>::infinity()) ||
95 (distance_[w] + weight != distance_[w]));
96
97 std::vector<double> cost(boost::num_vertices(*graph_),
98 std::numeric_limits<double>::infinity()); // initialize to n values of cost oo
99
100 IsLessThan isLessThan(cost);
101 Queue queue(isLessThan);
102
103 if (distance_[v] + weight < distance_[w])
104 {
105 distance_[w] = distance_[v] + weight;
106 parent_[w] = v;
107
108 cost[w] = 0;
109 queue.insert(w);
110 }
111
112 WeightMap weights = boost::get(boost::edge_weight_t(), *graph_);
113 while (!queue.empty())
114 {
115 // pop head of queue
116 std::size_t u = *(queue.begin());
117 queue.erase(queue.begin());
118
119 if (collectVertices)
120 affectedVertices.push_back(u);
121
122 boost::out_edges(u, *graph_);
123
124 // for every outgoing edge, see if we can improve its cost
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)
127 {
128 std::size_t x = boost::target(*ei, *graph_);
129 double edgeWeight = boost::get(weights, *ei);
130
131 if (distance_[u] + edgeWeight < distance_[x])
132 {
133 distance_[x] = distance_[u] + edgeWeight;
134 parent_[x] = u;
135
136 // insert to queue
137 auto qIter = queue.find(x);
138 if (qIter != queue.end())
139 queue.erase(qIter);
140
141 cost[x] = distance_[x] - distance_[v];
142 queue.insert(x);
143 }
144 }
145 }
146 }
147
148 void removeEdge(std::size_t v, std::size_t w, bool collectVertices, std::list<std::size_t> &affectedVertices)
149 {
150 // first, remove edge from graph
151 boost::remove_edge(v, w, *graph_);
152 if (parent_[w] != v)
153 return;
154
155 // Phase 1: Identify the affected vertices and remove the affected edges from SP(G)
156 std::list<std::size_t> workSet;
157 IntSet affectedVerticesSet;
158 workSet.push_back(w);
159
160 while (!workSet.empty())
161 {
162 // S elect and remove a vertex u from WorkSet
163 std::size_t u = workSet.front();
164 workSet.pop_front();
165
166 affectedVerticesSet.insert(u);
167
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)
170 {
171 std::size_t x = boost::target(*ei, *graph_);
172 if (parent_[x] == u)
173 workSet.push_back(x);
174 }
175 }
176
177 WeightMap weights = boost::get(boost::edge_weight_t(), *graph_);
178
179 // Phase 2: Determine new distances from affected vertices to source(G) and update SP(G).
180 IsLessThan isLessThan(distance_);
181 Queue queue(isLessThan);
182 for (auto set_iter = affectedVerticesSet.begin(); set_iter != affectedVerticesSet.end(); ++set_iter)
183 {
184 std::size_t a = *set_iter;
185 distance_[a] = std::numeric_limits<double>::infinity();
186
187 // go over all incoming neighbors which are NOT affected vertices
188 // get the best such neighbor
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)
191 {
192 std::size_t b = boost::source(*ei, *graph_);
193 if (affectedVerticesSet.find(b) == affectedVerticesSet.end())
194 {
195 double edgeWeight = boost::get(weights, *ei);
196
197 if (distance_[b] + edgeWeight < distance_[a])
198 {
199 distance_[a] = distance_[b] + edgeWeight;
200 parent_[a] = b;
201 }
202 }
203 }
204 if (distance_[a] != std::numeric_limits<double>::infinity())
205 queue.insert(a);
206 }
207
208 while (!queue.empty())
209 {
210 // pop head of queue
211 std::size_t a = *queue.begin();
212 queue.erase(queue.begin());
213
214 if (collectVertices)
215 affectedVertices.push_back(a);
216
217 // for every outgoing edge, see if we can improve its cost
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)
220 {
221 int c = boost::target(*ei, *graph_);
222 double edgeWeight = boost::get(weights, *ei);
223
224 if (distance_[a] + edgeWeight < distance_[c])
225 {
226 distance_[c] = distance_[a] + edgeWeight;
227 parent_[c] = a;
228
229 // insert to queue
230 auto qIter = queue.find(c);
231 if (qIter != queue.end())
232 queue.erase(qIter);
233
234 queue.insert(c);
235 }
236 }
237 }
238 }
239
240 double getShortestPathCost(std::size_t u) const
241 {
242 return this->distance_[u];
243 }
244
245 std::size_t getShortestPathParent(std::size_t u) const
246 {
247 return parent_[u];
248 }
249
250 private:
251 using WeightProperty = boost::property<boost::edge_weight_t, double>;
252 using Graph = boost::adjacency_list<boost::vecS, // container type for the edge list
253 boost::vecS, // container type for the vertex list
254 boost::bidirectionalS, // directedS / undirectedS / bidirectionalS
255 std::size_t, // vertex properties
256 WeightProperty>; // edge properties
257 using WeightMap = boost::property_map<Graph, boost::edge_weight_t>::type;
258
259 static const int NO_ID = -1;
260
261 class IsLessThan
262 {
263 public:
264 IsLessThan(std::vector<double> &cost) : cost_(cost)
265 {
266 }
267
268 bool operator()(std::size_t id1, std::size_t id2) const
269 {
270 return (cost_[id1] < cost_[id2]);
271 }
272
273 private:
274 std::vector<double> &cost_;
275 }; // IsLessThan
276
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;
281
282 Graph *graph_;
284 std::vector<double> distance_;
286 std::vector<std::size_t> parent_;
287 }; // DynamicSSSP
288} // namespace ompl
289
290#endif // OMPL_DATASTRUCTURES_DYNAMICSSSP_H
Main namespace. Contains everything in this library.