Gazebo Math

API Reference

7.5.1
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>
29 #include "gz/math/graph/Graph.hh"
30 #include "gz/math/Helpers.hh"
31 
32 namespace gz::math
33 {
34 // Inline bracket to help doxygen filtering.
35 inline namespace GZ_MATH_VERSION_NAMESPACE {
36 namespace 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.
55  Graph<bool, E, EdgeType> visitorGraph;
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 
65  std::vector<VertexId> visited;
66  std::list<VertexId> pending = {_from};
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.
107  Graph<bool, E, EdgeType> visitorGraph;
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 
117  std::vector<VertexId> visited;
118  std::stack<VertexId> pending({_from});
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));
247  dist[_from] = 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>
289  const UndirectedGraph<V, E> &_graph)
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)
300  visited[vId] = componentCount;
301  ++componentCount;
302  }
303  }
304 
305  std::vector<UndirectedGraph<V, E>> res(componentCount);
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_
iv>
357 #endif // GZ_MATH_GRAPH_GRAPHALGORITHMS_HH_