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
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/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 
55 namespace 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
ompl
Main namespace. Contains everything in this library.
Definition: MultiLevelPlanarManipulatorDemo.cpp:65