Gazebo Math

API Reference

8.1.0
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::math
33{
34// Inline bracket to help doxygen filtering.
35inline namespace GZ_MATH_VERSION_NAMESPACE {
36namespace graph
37{
42
49 template<typename V, typename E, typename EdgeType>
51 const VertexId &_from)
52 {
53 // Create an auxiliary graph, where the data is just a boolean value that
54 // stores whether the vertex has been visited or not.
56
57 // Copy the vertices (just the Id).
58 for (auto const &v : _graph.Vertices())
59 visitorGraph.AddVertex("", false, v.first);
60
61 // Copy the edges (without data).
62 for (auto const &e : _graph.Edges())
63 visitorGraph.AddEdge(e.second.get().Vertices(), E());
64
67
68 while (!pending.empty())
69 {
70 auto vId = pending.front();
71 pending.pop_front();
72
73 // If the vertex has been visited, skip.
74 auto &vertex = visitorGraph.VertexFromId(vId);
75 if (vertex.Data())
76 continue;
77
78 visited.push_back(vId);
79 vertex.Data() = true;
80
81 // Add more vertices to visit if they haven't been visited yet.
82 auto adjacents = visitorGraph.AdjacentsFrom(vId);
83 for (auto const &adj : adjacents)
84 {
85 vId = adj.first;
86 auto &v = adj.second.get();
87 if (!v.Data())
88 pending.push_back(vId);
89 }
90 }
91
92 return visited;
93 }
94
101 template<typename V, typename E, typename EdgeType>
103 const VertexId &_from)
104 {
105 // Create an auxiliary graph, where the data is just a boolean value that
106 // stores whether the vertex has been visited or not.
108
109 // Copy the vertices (just the Id).
110 for (auto const &v : _graph.Vertices())
111 visitorGraph.AddVertex("", false, v.first);
112
113 // Copy the edges (without data).
114 for (auto const &e : _graph.Edges())
115 visitorGraph.AddEdge(e.second.get().Vertices(), E());
116
119
120 while (!pending.empty())
121 {
122 auto vId = pending.top();
123 pending.pop();
124
125 // If the vertex has been visited, skip.
126 auto &vertex = visitorGraph.VertexFromId(vId);
127 if (vertex.Data())
128 continue;
129
130 visited.push_back(vId);
131 vertex.Data() = true;
132
133 // Add more vertices to visit if they haven't been visited yet.
134 auto adjacents = visitorGraph.AdjacentsFrom(vId);
135 for (auto const &adj : adjacents)
136 {
137 vId = adj.first;
138 auto &v = adj.second.get();
139 if (!v.Data())
140 pending.push(vId);
141 }
142 }
143
144 return visited;
145 }
146
210 template<typename V, typename E, typename EdgeType>
212 const VertexId &_from,
213 const VertexId &_to = kNullId)
214 {
215 auto allVertices = _graph.Vertices();
216
217 // Sanity check: The source vertex should exist.
218 if (allVertices.find(_from) == allVertices.end())
219 {
220 std::cerr << "Vertex [" << _from << "] Not found" << std::endl;
221 return {};
222 }
223
224 // Sanity check: The destination vertex should exist (if used).
225 if (_to != kNullId &&
226 allVertices.find(_to) == allVertices.end())
227 {
228 std::cerr << "Vertex [" << _from << "] Not found" << std::endl;
229 return {};
230 }
231
232 // Store vertices that are being preprocessed.
235
236 // Create a map for distances and next neightbor and initialize all
237 // distances as infinite.
239 for (auto const &v : allVertices)
240 {
241 auto id = v.first;
242 dist[id] = std::make_pair(MAX_D, kNullId);
243 }
244
245 // Insert _from in the priority queue and initialize its distance as 0.
246 pq.push(std::make_pair(0.0, _from));
248
249 while (!pq.empty())
250 {
251 // This is the minimum distance vertex.
252 VertexId u = pq.top().second;
253
254 // Shortcut: Destination vertex found, exiting.
255 if (_to != kNullId && _to == u)
256 break;
257
258 pq.pop();
259
260 for (auto const &edgePair : _graph.IncidentsFrom(u))
261 {
262 const auto &edge = edgePair.second.get();
263 const auto &v = edge.From(u);
264 double weight = edge.Weight();
265
266 // If there is shorted path to v through u.
267 if (dist[v].first > dist[u].first + weight)
268 {
269 // Updating distance of v.
270 dist[v] = std::make_pair(dist[u].first + weight, u);
271 pq.push(std::make_pair(dist[v].first, v));
272 }
273 }
274 }
275
276 return dist;
277 }
278
287 template<typename V, typename E>
290 {
292 unsigned int componentCount = 0;
293
294 for (auto const &v : _graph.Vertices())
295 {
296 if (visited.find(v.first) == visited.end())
297 {
298 auto component = BreadthFirstSort(_graph, v.first);
299 for (auto const &vId : component)
302 }
303 }
304
306
307 // Create the vertices.
308 for (auto const &vPair : _graph.Vertices())
309 {
310 const auto &v = vPair.second.get();
311 const auto &componentId = visited[v.Id()];
312 res[componentId].AddVertex(v.Name(), v.Data(), v.Id());
313 }
314
315 // Create the edges.
316 for (auto const &ePair : _graph.Edges())
317 {
318 const auto &e = ePair.second.get();
319 const auto &vertices = e.Vertices();
320 const auto &componentId = visited[vertices.first];
321 res[componentId].AddEdge(vertices, e.Data(), e.Weight());
322 }
323
324 return res;
325 }
326
332 template<typename V, typename E>
334 {
335 std::vector<Vertex<V>> vertices;
337
338 // Add all vertices.
339 for (auto const &vPair : _graph.Vertices())
340 {
341 // cppcheck-suppress useStlAlgorithm
342 vertices.push_back(vPair.second.get());
343 }
344
345 // Add all edges.
346 for (auto const &ePair : _graph.Edges())
347 {
348 auto const &e = ePair.second.get();
349 edges.push_back({e.Vertices(), e.Data(), e.Weight()});
350 }
351
352 return UndirectedGraph<V, E>(vertices, edges);
353 }
354} // namespace graph
355} // namespace GZ_MATH_VERSION_NAMESPACE
356} // namespace gz::math
357#endif // GZ_MATH_GRAPH_GRAPHALGORITHMS_HH_