Gazebo Math

API Reference

8.0.0
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"
31
32namespace gz::math
33{
34// Inline bracket to help doxygen filtering.
35inline namespace GZ_MATH_VERSION_NAMESPACE {
36namespace graph
37{
46 //
99 template<typename V, typename E, typename EdgeType>
100 class Graph
101 {
103 public: Graph() = default;
104
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
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(
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 {
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 {
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
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
224 return this->LinkEdge(std::move(newEdge));
225 }
226
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 {
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
291 {
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);
304 {
305 const auto &neighborVertex = this->VertexFromId(neighborVertexId);
306 res.emplace(
308 }
309 }
310
311 return res;
312 }
313
330 {
331 return this->AdjacentsFrom(_vertex.Id());
332 }
333
350 {
351 auto incidentEdges = this->IncidentsTo(_vertex);
352
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(
362 }
363
364 return res;
365 }
366
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
425 const
426 {
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
450 const Vertex<V> &_vertex) const
451 {
452 return this->IncidentsFrom(_vertex.Id());
453 }
454
461 const VertexId &_vertex) const
462 {
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
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
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
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
639 const VertexId _sourceId, const VertexId _destId) const
640 {
641 // Get the adjacency iterator for the source vertex.
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
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>
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 = 0u;
732
734 protected: VertexId nextEdgeId = 0u;
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
751 };
752
755 template<typename VV, typename EE>
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>
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>
811
814 template<typename V, typename E>
816} // namespace graph
817} // namespace GZ_MATH_VERSION_NAMESPACE
818} // namespace gz::math::graph
819#endif // GZ_MATH_GRAPH_GRAPH_HH_