Gazebo Math
API Reference
8.1.0
insert_drive_file
Tutorials
library_books
Classes
toc
Namespaces
insert_drive_file
Files
launch
Gazebo Website
Index
List
Hierarchy
Members: All
Members: Functions
Members: Variables
Members: Typedefs
Members: Enumerations
Members: Enumerator
List
Members
Functions
Typedefs
Variables
Enumerations
Enumerator
src
gz-math
include
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
{
41
using
CostInfo
=
std::pair<double, VertexId>
;
42
49
template
<
typename
V,
typename
E,
typename
EdgeType>
50
std::vector<VertexId>
BreadthFirstSort
(
const
Graph<V, E, EdgeType>
&
_graph
,
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>
102
std::vector<VertexId>
DepthFirstSort
(
const
Graph<V, E, EdgeType>
&
_graph
,
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>
211
std::map<VertexId, CostInfo>
Dijkstra
(
const
Graph<V, E, EdgeType>
&
_graph
,
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.
233
std::priority_queue
<
CostInfo
,
234
std::vector<CostInfo>
,
std::greater<CostInfo>
>
pq
;
235
236
// Create a map for distances and next neightbor and initialize all
237
// distances as infinite.
238
std::map<VertexId, CostInfo>
dist
;
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>
288
std::vector<UndirectedGraph<V, E>
>
ConnectedComponents
(
289
const
UndirectedGraph<V, E>
&
_graph
)
290
{
291
std::map<VertexId, unsigned int>
visited
;
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>
333
UndirectedGraph<V, E>
ToUndirectedGraph
(
const
DirectedGraph<V, E>
&
_graph
)
334
{
335
std::vector<Vertex<V>
> vertices;
336
std::vector<EdgeInitializer<E>
> edges;
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_