Loading...
Searching...
No Matches
AdjacencyList.cpp
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2015, Rice 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 Rice 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: Ryan Luna */
36
37#include "ompl/datastructures/AdjacencyList.h"
38#include "ompl/datastructures/BinaryHeap.h"
39#include "ompl/util/Console.h"
40#include <boost/graph/adjacency_list.hpp>
41#include <boost/graph/graph_traits.hpp>
42#include <boost/graph/dijkstra_shortest_paths.hpp>
43#include <boost/pending/disjoint_sets.hpp>
44
45#include <boost/graph/incremental_components.hpp> // needed?
46
47#include <iostream>
48
49using Graph = boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
50 boost::property<boost::vertex_index_t, int,
51 boost::property<boost::vertex_rank_t, int, // disjoint sets
52 boost::property<boost::vertex_predecessor_t, int>>>,
53 boost::property<boost::edge_weight_t, double>>;
54
55using Vertex = boost::graph_traits<Graph>::vertex_descriptor;
56using Edge = boost::graph_traits<Graph>::edge_descriptor;
57using VIterator = boost::graph_traits<Graph>::vertex_iterator;
58using EIterator = boost::graph_traits<Graph>::edge_iterator;
59using AdjIterator = boost::graph_traits<Graph>::adjacency_iterator;
60
61using DisjointSets = boost::disjoint_sets<boost::property_map<Graph, boost::vertex_rank_t>::type,
62 boost::property_map<Graph, boost::vertex_predecessor_t>::type>;
63
64#define graph_ reinterpret_cast<Graph *>(graphRaw_)
65#define disjointSets_ reinterpret_cast<DisjointSets *>(disjointSetsRaw_)
66
67ompl::AdjacencyList::AdjacencyList()
68{
69 graphRaw_ = new Graph();
70 disjointSetsRaw_ =
71 new DisjointSets(boost::get(boost::vertex_rank, *graph_), boost::get(boost::vertex_predecessor, *graph_));
72}
73
74ompl::AdjacencyList::AdjacencyList(int n)
75{
76 graphRaw_ = new Graph(n);
77 disjointSetsRaw_ =
78 new DisjointSets(boost::get(boost::vertex_rank, *graph_), boost::get(boost::vertex_predecessor, *graph_));
79
80 std::pair<VIterator, VIterator> vRange = boost::vertices(*graph_);
81 for (VIterator it = vRange.first; it != vRange.second; ++it)
82 disjointSets_->make_set(*it);
83}
84
85ompl::AdjacencyList::~AdjacencyList()
86{
87 delete graph_;
88 delete disjointSets_;
89 graphRaw_ = NULL;
90 disjointSetsRaw_ = NULL;
91}
92
93void ompl::AdjacencyList::clear()
94{
95 boost::mutex::scoped_lock lock(lock_);
96 graph_->clear();
97}
98
99int ompl::AdjacencyList::addVertex()
100{
101 boost::mutex::scoped_lock lock(lock_);
102 Vertex v = boost::add_vertex(*graph_);
103 disjointSets_->make_set(v);
104
105 boost::property_map<Graph, boost::vertex_index_t>::type vertexIndexMap = get(boost::vertex_index, *graph_);
106 return vertexIndexMap[v];
107}
108
109int ompl::AdjacencyList::numVertices() const
110{
111 return boost::num_vertices(*graph_);
112}
113
114bool ompl::AdjacencyList::vertexExists(int v) const
115{
116 return v >= 0 && v < numVertices();
117}
118
119bool ompl::AdjacencyList::inSameComponent(int v1, int v2) const
120{
121 if (!vertexExists(v1) || !vertexExists(v2))
122 return false;
123
124 return boost::same_component(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *disjointSets_);
125}
126
127int ompl::AdjacencyList::numConnectedComponents() const
128{
129 std::pair<VIterator, VIterator> vRange = boost::vertices(*graph_);
130 return disjointSets_->count_sets(vRange.first, vRange.second);
131}
132
133int ompl::AdjacencyList::getComponentID(int vtx) const
134{
135 return disjointSets_->find_set(vtx);
136}
137
138bool ompl::AdjacencyList::addEdge(int v1, int v2, double weight)
139{
140 boost::mutex::scoped_lock lock(lock_);
141
142 // If either of the vertices do not exist, don't add an edge
143 if (v1 < 0 || v1 >= numVertices() || v2 < 0 || v2 >= numVertices())
144 return false;
145
146 if (edgeExists(v1, v2))
147 return false;
148
149 // No self-transitions
150 if (v1 == v2)
151 return false;
152
153 if (weight < 0)
154 {
155 std::cout << "weight = " << weight << std::endl;
156 throw std::runtime_error("addEdge: Edge weight must be >= 0");
157 }
158
159 Edge e;
160 bool added = false;
161 Vertex vt1 = boost::vertex(v1, *graph_);
162 Vertex vt2 = boost::vertex(v2, *graph_);
163 tie(e, added) = boost::add_edge(vt1, vt2, *graph_);
164
165 assert(added); // this should never fail
166
167 // Set edge weight
168 boost::property_map<Graph, boost::edge_weight_t>::type weights = get(boost::edge_weight, *graph_);
169 weights[e] = weight;
170
171 disjointSets_->union_set(vt1, vt2);
172
173 return added;
174}
175
176bool ompl::AdjacencyList::removeEdge(int v1, int v2)
177{
178 boost::mutex::scoped_lock lock(lock_);
179
180 Edge e;
181 bool exists;
182 boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
183
184 if (exists)
185 boost::remove_edge(e, *graph_);
186 return exists;
187}
188
189int ompl::AdjacencyList::numEdges() const
190{
191 return boost::num_edges(*graph_);
192}
193
194double ompl::AdjacencyList::getEdgeWeight(int v1, int v2) const
195{
196 Edge e;
197 bool exists;
198 boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
199
200 if (exists)
201 {
202 boost::property_map<Graph, boost::edge_weight_t>::type weights = get(boost::edge_weight, *graph_);
203 return weights[e];
204 }
205
206 throw std::runtime_error("Edge does not exist");
207}
208
209bool ompl::AdjacencyList::setEdgeWeight(int v1, int v2, double weight)
210{
211 boost::mutex::scoped_lock lock(lock_);
212
213 if (weight < 0)
214 {
215 std::cout << "Weight: " << weight << std::endl;
216 throw std::runtime_error("setEdgeWeight: Edge weight must be >= 0");
217 }
218
219 Edge e;
220 bool exists;
221 boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
222
223 if (exists)
224 {
225 boost::property_map<Graph, boost::edge_weight_t>::type weights = get(boost::edge_weight, *graph_);
226 weights[e] = weight;
227 return true;
228 }
229
230 return false;
231}
232
233bool ompl::AdjacencyList::edgeExists(int v1, int v2) const
234{
235 Edge e;
236 bool exists;
237 boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
238
239 return exists;
240}
241
242int ompl::AdjacencyList::numNeighbors(int vtx) const
243{
244 return boost::degree(boost::vertex(vtx, *graph_), *graph_);
245}
246
247void ompl::AdjacencyList::getNeighbors(int vtx, std::vector<int> &nbrs) const
248{
249 nbrs.resize(numNeighbors(vtx));
250
251 std::pair<AdjIterator, AdjIterator> iterators = boost::adjacent_vertices(boost::vertex(vtx, *graph_), *graph_);
252 boost::property_map<Graph, boost::vertex_index_t>::type vertices = get(boost::vertex_index, *graph_);
253 int count = 0;
254 for (AdjIterator iter = iterators.first; iter != iterators.second; ++iter, ++count)
255 nbrs[count] = vertices[*iter];
256}
257
258void ompl::AdjacencyList::getNeighbors(int vtx, std::vector<std::pair<int, double>> &nbrs) const
259{
260 nbrs.resize(numNeighbors(vtx));
261
262 std::pair<AdjIterator, AdjIterator> iterators = boost::adjacent_vertices(boost::vertex(vtx, *graph_), *graph_);
263 boost::property_map<Graph, boost::vertex_index_t>::type vertices = get(boost::vertex_index, *graph_);
264 boost::property_map<Graph, boost::edge_weight_t>::type weights = get(boost::edge_weight, *graph_);
265
266 int count = 0;
267 for (AdjIterator iter = iterators.first; iter != iterators.second; ++iter, ++count)
268 {
269 Edge e;
270 bool exists;
271 boost::tie(e, exists) = boost::edge(boost::vertex(vtx, *graph_), *iter, *graph_);
272 assert(exists);
273 nbrs[count] = std::make_pair(vertices[*iter], weights[e]);
274 }
275}
276
277bool ompl::AdjacencyList::dijkstra(int v1, int v2, std::vector<int> &path) const
278{
279 std::vector<double> distances;
280 std::vector<int> predecessors;
281 dijkstra(v1, predecessors, distances);
282
283 // no solution when predecessor of (non-start) vertex is itself
284 if (v2 != v1 && predecessors[v2] == v2)
285 return false;
286
287 boost::property_map<Graph, boost::vertex_index_t>::type indexMap = get(boost::vertex_index, *graph_);
288
289 path.clear();
290 // Extracting solution path
291 Vertex v = boost::vertex(v2, *graph_);
292 Vertex start = boost::vertex(v1, *graph_);
293 while (v != start)
294 {
295 path.insert(path.begin(), indexMap[v]);
296 v = predecessors[v];
297 }
298 path.insert(path.begin(), indexMap[v]);
299 return true;
300}
301
302void ompl::AdjacencyList::dijkstra(int vtx, std::vector<int> &predecessors, std::vector<double> &distances) const
303{
304 // Locking the graph, since modifications to the graph during search are not supported
305 // This call is also the reason why lock_ must be mutable
306 boost::mutex::scoped_lock lock(lock_);
307
308 distances.resize(numVertices());
309 predecessors.resize(numVertices());
310
311 boost::dijkstra_shortest_paths(*graph_, boost::vertex(vtx, *graph_),
312 boost::predecessor_map(&predecessors[0]).distance_map(&distances[0]));
313}