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
Graph.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_GRAPH_HH_
18
#define GZ_MATH_GRAPH_GRAPH_HH_
19
20
#include <cassert>
21
#include <iostream>
22
#include <map>
23
#include <set>
24
#include <string>
25
#include <utility>
26
#include <vector>
27
28
#include <gz/math/config.hh>
29
#include "
gz/math/graph/Edge.hh
"
30
#include "
gz/math/graph/Vertex.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
{
46
//
99
template
<
typename
V,
typename
E,
typename
EdgeType>
100
class
Graph
101
{
103
public
:
Graph
() =
default
;
104
108
public
:
Graph
(
const
std::vector
<
Vertex<V>
> &
_vertices
,
109
const
std::vector
<
EdgeInitializer<E>
> &
_edges
)
110
{
111
// Add all vertices.
112
for
(
auto
const
&
v
:
_vertices
)
113
{
114
if
(!this->AddVertex(
v
.Name(),
v
.Data(),
v
.Id()).Valid())
115
{
116
std::cerr
<<
"Invalid vertex with Id ["
<<
v
.Id() <<
"]. Ignoring."
117
<<
std::endl
;
118
}
119
}
120
121
// Add all edges.
122
for
(
auto
const
&
e
:
_edges
)
123
{
124
if
(!this->AddEdge(
e
.vertices,
e
.data,
e
.weight).Valid())
125
std::cerr
<<
"Ignoring edge"
<<
std::endl
;
126
}
127
}
128
135
public
:
Vertex<V>
&
AddVertex
(
const
std::string
&
_name
,
136
const
V
&
_data
,
137
const
VertexId
&
_id
= kNullId)
138
{
139
auto
id
=
_id
;
140
141
// The user didn't provide an Id, we generate it.
142
if
(
id
==
kNullId
)
143
{
144
id
= this->NextVertexId();
145
146
// No space for new Ids.
147
if
(
id
==
kNullId
)
148
{
149
std::cerr
<<
"[Graph::AddVertex()] The limit of vertices has been "
150
<<
"reached. Ignoring vertex."
<<
std::endl
;
151
return
NullVertex<V>
();
152
}
153
}
154
155
// Create the vertex.
156
auto
ret
= this->vertices.insert(
157
std::make_pair
(
id
,
Vertex<V>
(
_name
,
_data
,
id
)));
158
159
// The Id already exists.
160
if
(!
ret
.second)
161
{
162
std::cerr
<<
"[Graph::AddVertex()] Repeated vertex ["
<<
id
<<
"]"
163
<<
std::endl
;
164
return
NullVertex<V>
();
165
}
166
167
// Link the vertex with an empty list of edges.
168
this->adjList[id] =
EdgeId_S
();
169
170
// Update the map of names.
171
this->names.insert(
std::make_pair
(
_name
,
id
));
172
173
return
ret
.first->second;
174
}
175
179
public
:
const
VertexRef_M<V>
Vertices
()
const
180
{
181
VertexRef_M<V>
res
;
182
for
(
auto
const
&
v
: this->vertices)
183
res
.emplace(
std::make_pair
(
v
.first,
std::cref
(
v
.second)));
184
185
return
res
;
186
}
187
191
public
:
const
VertexRef_M<V>
Vertices
(
const
std::string
&
_name
)
const
192
{
193
VertexRef_M<V>
res
;
194
for
(
auto
const
&
vertex
: this->vertices)
195
{
196
if
(
vertex
.second.Name() ==
_name
)
197
res
.emplace(
std::make_pair
(
vertex
.first,
std::cref
(
vertex
.second)));
198
}
199
200
return
res
;
201
}
202
209
public
:
EdgeType
&
AddEdge
(
const
VertexId_P
&
_vertices
,
210
const
E
&
_data
,
211
const
double
_weight
= 1.0)
212
{
213
auto
id
= this->NextEdgeId();
214
215
// No space for new Ids.
216
if
(
id
==
kNullId
)
217
{
218
std::cerr
<<
"[Graph::AddEdge()] The limit of edges has been reached. "
219
<<
"Ignoring edge."
<<
std::endl
;
220
return
NullEdge<E, EdgeType>
();
221
}
222
223
EdgeType
newEdge
(
_vertices
,
_data
,
_weight
,
id
);
224
return
this->LinkEdge(
std::move
(
newEdge
));
225
}
226
233
public
:
EdgeType
&
LinkEdge
(
const
EdgeType
&
_edge
)
234
{
235
auto
edgeVertices
=
_edge
.Vertices();
236
237
// Sanity check: Both vertices should exist.
238
for
(
auto
const
&
v
: {
edgeVertices
.first,
edgeVertices
.second})
239
{
240
if
(this->vertices.find(
v
) == this->vertices.end())
241
return
NullEdge<E, EdgeType>
();
242
}
243
244
// Link the new edge.
245
for
(
auto
const
&
v
: {
edgeVertices
.first,
edgeVertices
.second})
246
{
247
if
(
v
!=
kNullId
)
248
{
249
auto
vertexIt
= this->adjList.find(
v
);
250
assert
(
vertexIt
!= this->adjList.end());
251
vertexIt
->second.insert(
_edge
.Id());
252
}
253
}
254
255
auto
ret
= this->edges.insert(
std::make_pair
(
_edge
.Id(),
_edge
));
256
257
// Return the new edge.
258
return
ret
.first->second;
259
}
260
264
public
:
const
EdgeRef_M<EdgeType>
Edges
()
const
265
{
266
EdgeRef_M<EdgeType>
res
;
267
for
(
auto
const
&
edge
: this->edges)
268
{
269
res
.emplace(
std::make_pair
(
edge
.first,
std::cref
(
edge
.second)));
270
}
271
272
return
res
;
273
}
274
290
public
:
VertexRef_M<V>
AdjacentsFrom
(
const
VertexId
&
_vertex
)
const
291
{
292
VertexRef_M<V>
res
;
293
294
// Make sure the vertex exists
295
auto
vertexIt
= this->adjList.find(
_vertex
);
296
if
(
vertexIt
== this->adjList.end())
297
return
res
;
298
299
for
(
auto
const
&
edgeId
:
vertexIt
->second)
300
{
301
const
auto
&
edge
= this->EdgeFromId(
edgeId
);
302
auto
neighborVertexId
=
edge
.From(
_vertex
);
303
if
(
neighborVertexId
!=
kNullId
)
304
{
305
const
auto
&
neighborVertex
= this->VertexFromId(
neighborVertexId
);
306
res
.emplace(
307
std::make_pair
(
neighborVertexId
,
std::cref
(
neighborVertex
)));
308
}
309
}
310
311
return
res
;
312
}
313
329
public
:
VertexRef_M<V>
AdjacentsFrom
(
const
Vertex<V>
&
_vertex
)
const
330
{
331
return
this->AdjacentsFrom(
_vertex
.Id());
332
}
333
349
public
:
VertexRef_M<V>
AdjacentsTo
(
const
VertexId
&
_vertex
)
const
350
{
351
auto
incidentEdges
= this->IncidentsTo(
_vertex
);
352
353
VertexRef_M<V>
res
;
354
for
(
auto
const
&
incidentEdgeRef
:
incidentEdges
)
355
{
356
const
auto
&
incidentEdgeId
=
incidentEdgeRef
.first;
357
const
auto
&
incidentEdge
= this->EdgeFromId(
incidentEdgeId
);
358
const
auto
&
neighborVertexId
=
incidentEdge
.To(
_vertex
);
359
const
auto
&
neighborVertex
= this->VertexFromId(
neighborVertexId
);
360
res
.emplace(
361
std::make_pair
(
neighborVertexId
,
std::cref
(
neighborVertex
)));
362
}
363
364
return
res
;
365
}
366
382
public
:
VertexRef_M<V>
AdjacentsTo
(
const
Vertex<V>
&
_vertex
)
const
383
{
384
return
this->AdjacentsTo(
_vertex
.Id());
385
}
386
390
public
:
size_t
InDegree
(
const
VertexId
&
_vertex
)
const
391
{
392
return
this->IncidentsTo(
_vertex
).size();
393
}
394
398
public
:
size_t
InDegree
(
const
Vertex<V>
&
_vertex
)
const
399
{
400
return
this->IncidentsTo(this->VertexFromId(
_vertex
.Id())).size();
401
}
402
406
public
:
size_t
OutDegree
(
const
VertexId
&
_vertex
)
const
407
{
408
return
this->IncidentsFrom(
_vertex
).size();
409
}
410
414
public
:
size_t
OutDegree
(
const
Vertex<V>
&
_vertex
)
const
415
{
416
return
this->IncidentsFrom(this->VertexFromId(
_vertex
.Id())).size();
417
}
418
424
public
:
const
EdgeRef_M<EdgeType>
IncidentsFrom
(
const
VertexId
&
_vertex
)
425
const
426
{
427
EdgeRef_M<EdgeType>
res
;
428
429
const
auto
&
adjIt
= this->adjList.find(
_vertex
);
430
if
(
adjIt
== this->adjList.end())
431
return
res
;
432
433
const
auto
&
edgeIds
=
adjIt
->second;
434
for
(
auto
const
&
edgeId
:
edgeIds
)
435
{
436
const
auto
&
edge
= this->EdgeFromId(
edgeId
);
437
if
(
edge
.From(
_vertex
) !=
kNullId
)
438
res
.emplace(
std::make_pair
(
edge
.Id(),
std::cref
(
edge
)));
439
}
440
441
return
res
;
442
}
443
449
public
:
const
EdgeRef_M<EdgeType>
IncidentsFrom
(
450
const
Vertex<V>
&
_vertex
)
const
451
{
452
return
this->IncidentsFrom(
_vertex
.Id());
453
}
454
460
public
:
const
EdgeRef_M<EdgeType>
IncidentsTo
(
461
const
VertexId
&
_vertex
)
const
462
{
463
EdgeRef_M<EdgeType>
res
;
464
465
const
auto
&
adjIt
= this->adjList.find(
_vertex
);
466
if
(
adjIt
== this->adjList.end())
467
return
res
;
468
469
const
auto
&
edgeIds
=
adjIt
->second;
470
for
(
auto
const
&
edgeId
:
edgeIds
)
471
{
472
const
auto
&
edge
= this->EdgeFromId(
edgeId
);
473
if
(
edge
.To(
_vertex
) !=
kNullId
)
474
res
.emplace(
std::make_pair
(
edge
.Id(),
std::cref
(
edge
)));
475
}
476
477
return
res
;
478
}
479
485
public
:
const
EdgeRef_M<EdgeType>
IncidentsTo
(
const
Vertex<V>
&
_vertex
)
486
const
487
{
488
return
this->IncidentsTo(
_vertex
.Id());
489
}
490
494
public
:
bool
Empty
()
const
495
{
496
return
this->vertices.empty();
497
}
498
502
public
:
bool
RemoveVertex
(
const
VertexId
&
_vertex
)
503
{
504
auto
vIt
= this->vertices.find(
_vertex
);
505
if
(
vIt
== this->vertices.end())
506
return
false
;
507
508
std::string
name =
vIt
->second.Name();
509
510
// Remove incident edges.
511
auto
incidents
= this->IncidentsTo(
_vertex
);
512
for
(
auto
edgePair
:
incidents
)
513
this->RemoveEdge(
edgePair
.first);
514
515
// Remove all outgoing edges.
516
incidents
= this->IncidentsFrom(
_vertex
);
517
for
(
auto
edgePair
:
incidents
)
518
this->RemoveEdge(
edgePair
.first);
519
520
// Remove the vertex (key) from the adjacency list.
521
this->adjList.erase(
_vertex
);
522
523
// Remove the vertex.
524
this->vertices.erase(
_vertex
);
525
526
// Get an iterator to all vertices sharing name.
527
auto
iterPair
= this->names.equal_range(name);
528
for
(
auto
it
=
iterPair
.first;
it
!=
iterPair
.second; ++
it
)
529
{
530
if
(
it
->second ==
_vertex
)
531
{
532
this->names.erase(
it
);
533
break
;
534
}
535
}
536
537
return
true
;
538
}
539
543
public
:
bool
RemoveVertex
(
Vertex<V>
&
_vertex
)
544
{
545
return
this->RemoveVertex(
_vertex
.Id());
546
}
547
551
public
:
size_t
RemoveVertices
(
const
std::string
&
_name
)
552
{
553
size_t
numVertices
= this->names.count(
_name
);
554
size_t
result
= 0;
555
for
(
size_t
i
= 0;
i
<
numVertices
; ++
i
)
556
{
557
auto
iter = this->names.find(
_name
);
558
if
(this->RemoveVertex(iter->second))
559
++
result
;
560
}
561
562
return
result
;
563
}
564
570
public
:
bool
RemoveEdge
(
const
EdgeId
&
_edge
)
571
{
572
auto
edgeIt
= this->edges.find(
_edge
);
573
if
(
edgeIt
== this->edges.end())
574
return
false
;
575
576
auto
edgeVertices
=
edgeIt
->second.Vertices();
577
578
// Unlink the edge.
579
for
(
auto
const
&
v
: {
edgeVertices
.first,
edgeVertices
.second})
580
{
581
if
(
edgeIt
->second.From(
v
) !=
kNullId
)
582
{
583
auto
vertex
= this->adjList.find(
v
);
584
assert
(
vertex
!= this->adjList.end());
585
vertex
->second.erase(
_edge
);
586
}
587
}
588
589
this->edges.erase(
_edge
);
590
591
return
true
;
592
}
593
599
public
:
bool
RemoveEdge
(
EdgeType
&
_edge
)
600
{
601
return
this->RemoveEdge(
_edge
.Id());
602
}
603
608
public
:
const
Vertex<V>
&
VertexFromId
(
const
VertexId
&
_id
)
const
609
{
610
auto
iter = this->vertices.find(
_id
);
611
if
(iter == this->vertices.end())
612
return
NullVertex<V>
();
613
614
return
iter->second;
615
}
616
621
public
:
Vertex<V>
&
VertexFromId
(
const
VertexId
&
_id
)
622
{
623
auto
iter = this->vertices.find(
_id
);
624
if
(iter == this->vertices.end())
625
return
NullVertex<V>
();
626
627
return
iter->second;
628
}
629
638
public
:
const
EdgeType
&
EdgeFromVertices
(
639
const
VertexId
_sourceId
,
const
VertexId
_destId
)
const
640
{
641
// Get the adjacency iterator for the source vertex.
642
const
typename
std::map<VertexId, EdgeId_S>::const_iterator
&
adjIt
=
643
this->adjList.find(
_sourceId
);
644
645
// Quit early if there is no adjacency entry
646
if
(
adjIt
== this->adjList.end())
647
return
NullEdge<E, EdgeType>
();
648
649
// Loop over the edges in the source vertex's adjacency list
650
for
(
std::set<EdgeId>::const_iterator
edgIt
=
adjIt
->second.begin();
651
edgIt
!=
adjIt
->second.end(); ++
edgIt
)
652
{
653
// Get an iterator to the actual edge
654
const
typename
std::map<EdgeId, EdgeType>::const_iterator
edgeIter
=
655
this->edges.find(*
edgIt
);
656
657
// Check if the edge has the correct source and destination.
658
if
(
edgeIter
!= this->edges.end() &&
659
edgeIter
->second.From(
_sourceId
) ==
_destId
)
660
{
661
assert
(
edgeIter
->second.To(
_destId
) ==
_sourceId
);
662
return
edgeIter
->second;
663
}
664
}
665
666
return
NullEdge<E, EdgeType>
();
667
}
668
673
public
:
const
EdgeType
&
EdgeFromId
(
const
EdgeId
&
_id
)
const
674
{
675
auto
iter = this->edges.find(
_id
);
676
if
(iter == this->edges.end())
677
return
NullEdge<E, EdgeType>
();
678
679
return
iter->second;
680
}
681
686
public
:
EdgeType
&
EdgeFromId
(
const
EdgeId
&
_id
)
687
{
688
auto
iter = this->edges.find(
_id
);
689
if
(iter == this->edges.end())
690
return
NullEdge<E, EdgeType>
();
691
692
return
iter->second;
693
}
694
700
public
:
template
<
typename
VV,
typename
EE,
typename
EEdgeType>
701
friend
std::ostream
&
operator<<
(
std::ostream
&
_out
,
702
const
Graph<VV, EE, EEdgeType>
&
_g
);
703
706
private
:
VertexId
&NextVertexId()
707
{
708
while
(this->vertices.find(
this
->nextVertexId) !=
this
->vertices.end()
709
&&
this
->nextVertexId < MAX_UI64)
710
{
711
++this->nextVertexId;
712
}
713
714
return
this->nextVertexId;
715
}
716
719
private
: VertexId &NextEdgeId()
720
{
721
while
(this->edges.find(
this
->nextEdgeId) !=
this
->edges.end() &&
722
this
->nextEdgeId < MAX_UI64)
723
{
724
++this->nextEdgeId;
725
}
726
727
return
this->nextEdgeId;
728
}
729
731
protected
:
VertexId
nextVertexId = 0
u
;
732
734
protected
:
VertexId
nextEdgeId = 0
u
;
735
737
private
:
std::map<VertexId, Vertex<V>
> vertices;
738
740
private
:
std::map<EdgeId, EdgeType>
edges;
741
747
private
:
std::map<VertexId, EdgeId_S>
adjList;
748
750
private
:
std::multimap<std::string, VertexId>
names;
751
};
752
755
template
<
typename
VV,
typename
EE>
756
std::ostream
&
operator<<
(
std::ostream
&
_out
,
757
const
Graph
<
VV
,
EE
,
UndirectedEdge<EE>
> &
_g
)
758
{
759
_out
<<
"graph {"
<<
std::endl
;
760
761
// All vertices with the name and Id as a "label" attribute.
762
for
(
auto
const
&
vertexMap
:
_g
.Vertices())
763
{
764
auto
vertex
=
vertexMap
.second.get();
765
_out
<<
vertex
;
766
}
767
768
// All edges.
769
for
(
auto
const
&
edgeMap
:
_g
.Edges())
770
{
771
auto
edge
=
edgeMap
.second.get();
772
_out
<<
edge
;
773
}
774
775
_out
<<
"}"
<<
std::endl
;
776
777
return
_out
;
778
}
779
782
template
<
typename
VV,
typename
EE>
783
std::ostream
&
operator<<
(
std::ostream
&
_out
,
784
const
Graph
<
VV
,
EE
,
DirectedEdge<EE>
> &
_g
)
785
{
786
_out
<<
"digraph {"
<<
std::endl
;
787
788
// All vertices with the name and Id as a "label" attribute.
789
for
(
auto
const
&
vertexMap
:
_g
.Vertices())
790
{
791
auto
vertex
=
vertexMap
.second.get();
792
_out
<<
vertex
;
793
}
794
795
// All edges.
796
for
(
auto
const
&
edgeMap
:
_g
.Edges())
797
{
798
auto
edge
=
edgeMap
.second.get();
799
_out
<<
edge
;
800
}
801
802
_out
<<
"}"
<<
std::endl
;
803
804
return
_out
;
805
}
806
809
template
<
typename
V,
typename
E>
810
using
UndirectedGraph
=
Graph<V, E, UndirectedEdge<E>
>;
811
814
template
<
typename
V,
typename
E>
815
using
DirectedGraph
=
Graph<V, E, DirectedEdge<E>
>;
816
}
// namespace graph
817
}
// namespace GZ_MATH_VERSION_NAMESPACE
818
}
// namespace gz::math::graph
819
#endif
// GZ_MATH_GRAPH_GRAPH_HH_