Gazebo Math

API Reference

7.4.0
gz/math/graph/GraphAlgorithms.hh
Go to the documentation of this file.
1/*
2 * Copyright (C) 2017 Open Source Robotics Foundation
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *
16*/
17#ifndef GZ_MATH_GRAPH_GRAPHALGORITHMS_HH_
18#define GZ_MATH_GRAPH_GRAPHALGORITHMS_HH_
19
20#include <functional>
21#include <list>
22#include <map>
23#include <queue>
24#include <stack>
25#include <utility>
26#include <vector>
27
28#include <gz/math/config.hh>
30#include "gz/math/Helpers.hh"
31
32namespace gz
33{
34namespace math
35{
36// Inline bracket to help doxygen filtering.
37inline namespace GZ_MATH_VERSION_NAMESPACE {
38namespace graph
39{
44
51 template<typename V, typename E, typename EdgeType>
53 const VertexId &_from)
54 {
55 // Create an auxiliary graph, where the data is just a boolean value that
56 // stores whether the vertex has been visited or not.
57 Graph<bool, E, EdgeType> visitorGraph;
58
59 // Copy the vertices (just the Id).
60 for (auto const &v : _graph.Vertices())
61 visitorGraph.AddVertex("", false, v.first);
62
63 // Copy the edges (without data).
64 for (auto const &e : _graph.Edges())
65 visitorGraph.AddEdge(e.second.get().Vertices(), E());
66
68 std::list<VertexId> pending = {_from};
69
70 while (!pending.empty())
71 {
72 auto vId = pending.front();
73 pending.pop_front();
74
75 // If the vertex has been visited, skip.
76 auto &vertex = visitorGraph.VertexFromId(vId);
77 if (vertex.Data())
78 continue;
79
80 visited.push_back(vId);
81 vertex.Data() = true;
82
83 // Add more vertices to visit if they haven't been visited yet.
84 auto adjacents = visitorGraph.AdjacentsFrom(vId);
85 for (auto const &adj : adjacents)
86 {
87 vId = adj.first;
88 auto &v = adj.second.get();
89 if (!v.Data())
90 pending.push_back(vId);
91 }
92 }
93
94 return visited;
95 }
96
103 template<typename V, typename E, typename EdgeType>
105 const VertexId &_from)
106 {
107 // Create an auxiliary graph, where the data is just a boolean value that
108 // stores whether the vertex has been visited or not.
109 Graph<bool, E, EdgeType> visitorGraph;
110
111 // Copy the vertices (just the Id).
112 for (auto const &v : _graph.Vertices())
113 visitorGraph.AddVertex("", false, v.first);
114
115 // Copy the edges (without data).
116 for (auto const &e : _graph.Edges())
117 visitorGraph.AddEdge(e.second.get().Vertices(), E());
118
119 std::vector<VertexId> visited;
120 std::stack<VertexId> pending({_from});
121
122 while (!pending.empty())
123 {
124 auto vId = pending.top();
125 pending.pop();
126
127 // If the vertex has been visited, skip.
128 auto &vertex = visitorGraph.VertexFromId(vId);
129 if (vertex.Data())
130 continue;
131
132 visited.push_back(vId);
133 vertex.Data() = true;
134
135 // Add more vertices to visit if they haven't been visited yet.
136 auto adjacents = visitorGraph.AdjacentsFrom(vId);
137 for (auto const &adj : adjacents)
138 {
139 vId = adj.first;
140 auto &v = adj.second.get();
141 if (!v.Data())
142 pending.push(vId);
143 }
144 }
145
146 return visited;
147 }
148
212 template<typename V, typename E, typename EdgeType>
214 const VertexId &_from,
215 const VertexId &_to = kNullId)
216 {
217 auto allVertices = _graph.Vertices();
218
219 // Sanity check: The source vertex should exist.
220 if (allVertices.find(_from) == allVertices.end())
221 {
222 std::cerr << "Vertex [" << _from << "] Not found" << std::endl;
223 return {};
224 }
225
226 // Sanity check: The destination vertex should exist (if used).
227 if (_to != kNullId &&
228 allVertices.find(_to) == allVertices.end())
229 {
230 std::cerr << "Vertex [" << _from << "] Not found" << std::endl;
231 return {};
232 }
233
234 // Store vertices that are being preprocessed.
237
238 // Create a map for distances and next neightbor and initialize all
239 // distances as infinite.
241 for (auto const &v : allVertices)
242 {
243 auto id = v.first;
244 dist[id] = std::make_pair(MAX_D, kNullId);
245 }
246
247 // Insert _from in the priority queue and initialize its distance as 0.
248 pq.push(std::make_pair(0.0, _from));
249 dist[_from] = std::make_pair(0.0, _from);
250
251 while (!pq.empty())
252 {
253 // This is the minimum distance vertex.
254 VertexId u = pq.top().second;
255
256 // Shortcut: Destination vertex found, exiting.
257 if (_to != kNullId && _to == u)
258 break;
259
260 pq.pop();
261
262 for (auto const &edgePair : _graph.IncidentsFrom(u))
263 {
264 const auto &edge = edgePair.second.get();
265 const auto &v = edge.From(u);
266 double weight = edge.Weight();
267
268 // If there is shorted path to v through u.
269 if (dist[v].first > dist[u].first + weight)
270 {
271 // Updating distance of v.
272 dist[v] = std::make_pair(dist[u].first + weight, u);
273 pq.push(std::make_pair(dist[v].first, v));
274 }
275 }
276 }
277
278 return dist;
279 }
280
289 template<typename V, typename E>
291 const UndirectedGraph<V, E> &_graph)
292 {
294 unsigned int componentCount = 0;
295
296 for (auto const &v : _graph.Vertices())
297 {
298 if (visited.find(v.first) == visited.end())
299 {
300 auto component = BreadthFirstSort(_graph, v.first);
301 for (auto const &vId : component)
302 visited[vId] = componentCount;
303 ++componentCount;
304 }
305 }
306
307 std::vector<UndirectedGraph<V, E>> res(componentCount);
308
309 // Create the vertices.
310 for (auto const &vPair : _graph.Vertices())
311 {
312 const auto &v = vPair.second.get();
313 const auto &componentId = visited[v.Id()];
314 res[componentId].AddVertex(v.Name(), v.Data(), v.Id());
315 }
316
317 // Create the edges.
318 for (auto const &ePair : _graph.Edges())
319 {
320 const auto &e = ePair.second.get();
321 const auto &vertices = e.Vertices();
322 const auto &componentId = visited[vertices.first];
323 res[componentId].AddEdge(vertices, e.Data(), e.Weight());
324 }
325
326 return res;
327 }
328
334 template<typename V, typename E>
336 {
337 std::vector<Vertex<V>> vertices;
339
340 // Add all vertices.
341 for (auto const &vPair : _graph.Vertices())
342 {
343 // cppcheck-suppress useStlAlgorithm
344 vertices.push_back(vPair.second.get());
345 }
346
347 // Add all edges.
348 for (auto const &ePair : _graph.Edges())
349 {
350 auto const &e = ePair.second.get();
351 edges.push_back({e.Vertices(), e.Data(), e.Weight()});
352 }
353
354 return UndirectedGraph<V, E>(vertices, edges);
355 }
356}
357}
358}
359}
360#endif