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 */
36 /* Implementation based on
37 G. Ramalingam and T. W. Reps, On the computational complexity of
38 dynamic graph problems, Theor. Comput. Sci., vol. 158, no. 1&2, pp.
39 233-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/graph/graph_traits.hpp>
51 #include <boost/graph/adjacency_list.hpp>
52 #include <unordered_set>
53 
54 namespace ompl
55 {
56  class DynamicSSSP
57  {
58  public:
59  DynamicSSSP()
60  {
61  graph_ = new Graph(0);
62  }
63  ~DynamicSSSP(void)
64  {
65  delete graph_;
66  }
67 
68  void addVertex(std::size_t id)
69  {
70  distance_.push_back((id == 0) ? 0 : std::numeric_limits<double>::infinity());
71  parent_.push_back(NO_ID);
72  boost::add_vertex(id, *graph_);
73  return;
74  }
75 
76  // we assume that no two paths have the same cost,
77  // this asssumption is valid when the nodes have some randomeness to them
78  void addEdge(std::size_t v, std::size_t w, double weight,
79  bool collectVertices, std::list<std::size_t>& affectedVertices)
80  {
81  // first, add edge to graph
82  WeightProperty edge_property(weight);
83  boost::add_edge(v, w, edge_property, *graph_);
84 
85  // now, update distance_
86  assert ( (distance_[v] == std::numeric_limits<double>::infinity()) ||
87  (distance_[w] == std::numeric_limits<double>::infinity()) ||
88  (distance_[w] + weight != distance_[w]) );
89 
90  std::vector<double> cost( boost::num_vertices(*graph_),
91  std::numeric_limits<double>::infinity()); // initialize to n values of cost oo
92 
93  IsLessThan isLessThan(cost);
94  Queue queue(isLessThan);
95 
96  if (distance_[v] + weight < distance_[w])
97  {
98  distance_[w] = distance_[v] + weight;
99  parent_[w] = v;
100 
101  cost[w] = 0;
102  queue.insert(w);
103  }
104 
105  WeightMap weights = boost::get(boost::edge_weight_t(), *graph_);
106  while (!queue.empty())
107  {
108  // pop head of queue
109  std::size_t u = *(queue.begin());
110  queue.erase(queue.begin());
111 
112  if (collectVertices)
113  affectedVertices.push_back(u);
114 
115  boost::out_edges(u, *graph_);
116 
117  // for every outgoing edge, see if we can improve its cost
118  boost::graph_traits<Graph>::out_edge_iterator ei, ei_end;
119  for (boost::tie(ei, ei_end) = boost::out_edges(u, *graph_); ei != ei_end; ++ei)
120  {
121  std::size_t x = boost::target(*ei, *graph_);
122  double edgeWeight = boost::get(weights, *ei);
123 
124  if (distance_[u] + edgeWeight < distance_[x])
125  {
126  distance_[x] = distance_[u] + edgeWeight;
127  parent_[x] = u;
128 
129  // insert to queue
130  QueueIter qIter = queue.find(x);
131  if (qIter != queue.end() )
132  queue.erase(qIter);
133 
134  cost[x] = distance_[x] - distance_[v];
135  queue.insert(x);
136  }
137  }
138  }
139 
140  return;
141  }
142 
143  void removeEdge(std::size_t v, std::size_t w,
144  bool collectVertices, std::list<std::size_t>& affectedVertices)
145  {
146  // first, remove edge from graph
147  boost::remove_edge(v, w, *graph_);
148  if (parent_[w] != v)
149  return;
150 
151  // Phase 1: Identify the affected vertices and remove the affected edges from SP(G)
152  std::list<std::size_t> workSet;
153  IntSet affectedVerticesSet;
154  workSet.push_back(w);
155 
156  while (!workSet.empty())
157  {
158  //S elect and remove a vertex u from WorkSet
159  std::size_t u = workSet.front();
160  workSet.pop_front();
161 
162  affectedVerticesSet.insert(u);
163 
164  boost::graph_traits<Graph>::out_edge_iterator ei, ei_end;
165  for (boost::tie(ei, ei_end) = boost::out_edges(u, *graph_); ei != ei_end; ++ei)
166  {
167  std::size_t x = boost::target(*ei, *graph_);
168  if (parent_[x] == u)
169  workSet.push_back(x);
170  }
171  }
172 
173  WeightMap weights = boost::get(boost::edge_weight_t(), *graph_);
174 
175  // Phase 2: Determine new distances from affected vertices to source(G) and update SP(G).
176  IsLessThan isLessThan(distance_);
177  Queue queue(isLessThan);
178  for (IntSetIter set_iter = affectedVerticesSet.begin(); set_iter!= affectedVerticesSet.end(); ++set_iter)
179  {
180  std::size_t a = *set_iter;
181  distance_[a] = std::numeric_limits<double>::infinity();
182 
183  // go over all incoming neighbors which are NOT affected vertices
184  // get the best such neighbor
185  boost::graph_traits<Graph>::in_edge_iterator ei, ei_end;
186  for (boost::tie(ei, ei_end) = boost::in_edges(a, *graph_); ei != ei_end; ++ei)
187  {
188  std::size_t b = boost::source(*ei, *graph_);
189  if (affectedVerticesSet.find(b) == affectedVerticesSet.end())
190  {
191  double edgeWeight = boost::get(weights, *ei);
192 
193  if (distance_[b] + edgeWeight < distance_[a])
194  {
195  distance_[a] = distance_[b] + edgeWeight;
196  parent_[a] = b;
197  }
198  }
199  }
200  if (distance_[a] != std::numeric_limits<double>::infinity())
201  queue.insert(a);
202  }
203 
204  while(!queue.empty())
205  {
206  // pop head of queue
207  std::size_t a = *queue.begin();
208  queue.erase(queue.begin());
209 
210  if (collectVertices)
211  affectedVertices.push_back(a);
212 
213  // for every outgoing edge, see if we can improve its cost
214  boost::graph_traits<Graph>::out_edge_iterator ei, ei_end;
215  for (boost::tie(ei, ei_end) = boost::out_edges(a, *graph_); ei != ei_end; ++ei)
216  {
217  int c = boost::target(*ei, *graph_);
218  double edgeWeight = boost::get(weights, *ei);
219 
220  if (distance_[a] + edgeWeight < distance_[c])
221  {
222  distance_[c] = distance_[a] + edgeWeight;
223  parent_[c] = a;
224 
225  // insert to queue
226  QueueIter qIter = queue.find(c);
227  if (qIter != queue.end() )
228  queue.erase(qIter);
229 
230  queue.insert(c);
231  }
232  }
233  }
234 
235  return;
236  }
237 
238  double getShortestPathCost(std::size_t u) const
239  {
240  return this->distance_[u];
241  }
242 
243  std::size_t getShortestPathParent(std::size_t u) const
244  {
245  return parent_[u];
246  }
247  private:
248  typedef boost::property<boost::edge_weight_t, double> WeightProperty;
249  typedef boost::adjacency_list<boost::vecS, // container type for the edge list
250  boost::vecS, // container type for the vertex list
251  boost::bidirectionalS, // directedS / undirectedS / bidirectionalS
252  std::size_t, // vertex properties
253  WeightProperty // edge properties
254  > Graph;
255  typedef boost::property_map<Graph, boost::edge_weight_t>::type WeightMap;
256 
257  static const int NO_ID = -1;
258 
259  class IsLessThan
260  {
261  public:
262  IsLessThan(std::vector<double>& cost)
263  :cost_(cost)
264  {
265  }
266 
267  bool operator()(std::size_t id1, std::size_t id2) const
268  {
269  return (cost_[id1] < cost_[id2]);
270  }
271  private:
272  std::vector<double>& cost_;
273  }; //IsLessThan
274 
275  typedef std::set<std::size_t, IsLessThan> Queue;
276  typedef Queue::iterator QueueIter;
277  typedef std::unordered_set<std::size_t> IntSet;
278  typedef IntSet::iterator IntSetIter;
279 
280  Graph* graph_;
282  std::vector<double> distance_;
284  std::vector<std::size_t> parent_;
285  }; //DynamicSSSP
286 }
287 
288 #endif //OMPL_DATASTRUCTURES_DYNAMICSSSP_H
ompl
Main namespace. Contains everything in this library.
Definition: Cost.h:42