Gazebo Math

API Reference

7.4.0
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"
31
32namespace gz
33{
34namespace math
35{
36// Inline bracket to help doxygen filtering.
37inline namespace GZ_MATH_VERSION_NAMESPACE {
38namespace graph
39{
48 //
101 template<typename V, typename E, typename EdgeType>
102 class Graph
103 {
105 public: Graph() = default;
106
110 public: Graph(const std::vector<Vertex<V>> &_vertices,
111 const std::vector<EdgeInitializer<E>> &_edges)
112 {
113 // Add all vertices.
114 for (auto const &v : _vertices)
115 {
116 if (!this->AddVertex(v.Name(), v.Data(), v.Id()).Valid())
117 {
118 std::cerr << "Invalid vertex with Id [" << v.Id() << "]. Ignoring."
119 << std::endl;
120 }
121 }
122
123 // Add all edges.
124 for (auto const &e : _edges)
125 {
126 if (!this->AddEdge(e.vertices, e.data, e.weight).Valid())
127 std::cerr << "Ignoring edge" << std::endl;
128 }
129 }
130
137 public: Vertex<V> &AddVertex(const std::string &_name,
138 const V &_data,
139 const VertexId &_id = kNullId)
140 {
141 auto id = _id;
142
143 // The user didn't provide an Id, we generate it.
144 if (id == kNullId)
145 {
146 id = this->NextVertexId();
147
148 // No space for new Ids.
149 if (id == kNullId)
150 {
151 std::cerr << "[Graph::AddVertex()] The limit of vertices has been "
152 << "reached. Ignoring vertex." << std::endl;
154 }
155 }
156
157 // Create the vertex.
158 auto ret = this->vertices.insert(
159 std::make_pair(id, Vertex<V>(_name, _data, id)));
160
161 // The Id already exists.
162 if (!ret.second)
163 {
164 std::cerr << "[Graph::AddVertex()] Repeated vertex [" << id << "]"
165 << std::endl;
167 }
168
169 // Link the vertex with an empty list of edges.
170 this->adjList[id] = EdgeId_S();
171
172 // Update the map of names.
173 this->names.insert(std::make_pair(_name, id));
174
175 return ret.first->second;
176 }
177
181 public: const VertexRef_M<V> Vertices() const
182 {
183 VertexRef_M<V> res;
184 for (auto const &v : this->vertices)
185 res.emplace(std::make_pair(v.first, std::cref(v.second)));
186
187 return res;
188 }
189
193 public: const VertexRef_M<V> Vertices(const std::string &_name) const
194 {
195 VertexRef_M<V> res;
196 for (auto const &vertex : this->vertices)
197 {
198 if (vertex.second.Name() == _name)
199 res.emplace(std::make_pair(vertex.first, std::cref(vertex.second)));
200 }
201
202 return res;
203 }
204
211 public: EdgeType &AddEdge(const VertexId_P &_vertices,
212 const E &_data,
213 const double _weight = 1.0)
214 {
215 auto id = this->NextEdgeId();
216
217 // No space for new Ids.
218 if (id == kNullId)
219 {
220 std::cerr << "[Graph::AddEdge()] The limit of edges has been reached. "
221 << "Ignoring edge." << std::endl;
222 return EdgeType::NullEdge;
223 }
224
225 EdgeType newEdge(_vertices, _data, _weight, id);
226 return this->LinkEdge(std::move(newEdge));
227 }
228
235 public: EdgeType &LinkEdge(const EdgeType &_edge)
236 {
237 auto edgeVertices = _edge.Vertices();
238
239 // Sanity check: Both vertices should exist.
240 for (auto const &v : {edgeVertices.first, edgeVertices.second})
241 {
242 if (this->vertices.find(v) == this->vertices.end())
243 return EdgeType::NullEdge;
244 }
245
246 // Link the new edge.
247 for (auto const &v : {edgeVertices.first, edgeVertices.second})
248 {
249 if (v != kNullId)
250 {
251 auto vertexIt = this->adjList.find(v);
252 assert(vertexIt != this->adjList.end());
253 vertexIt->second.insert(_edge.Id());
254 }
255 }
256
257 auto ret = this->edges.insert(std::make_pair(_edge.Id(), _edge));
258
259 // Return the new edge.
260 return ret.first->second;
261 }
262
266 public: const EdgeRef_M<EdgeType> Edges() const
267 {
269 for (auto const &edge : this->edges)
270 {
271 res.emplace(std::make_pair(edge.first, std::cref(edge.second)));
272 }
273
274 return res;
275 }
276
292 public: VertexRef_M<V> AdjacentsFrom(const VertexId &_vertex) const
293 {
294 VertexRef_M<V> res;
295
296 // Make sure the vertex exists
297 auto vertexIt = this->adjList.find(_vertex);
298 if (vertexIt == this->adjList.end())
299 return res;
300
301 for (auto const &edgeId : vertexIt->second)
302 {
303 const auto &edge = this->EdgeFromId(edgeId);
304 auto neighborVertexId = edge.From(_vertex);
305 if (neighborVertexId != kNullId)
306 {
307 const auto &neighborVertex = this->VertexFromId(neighborVertexId);
308 res.emplace(
309 std::make_pair(neighborVertexId, std::cref(neighborVertex)));
310 }
311 }
312
313 return res;
314 }
315
331 public: VertexRef_M<V> AdjacentsFrom(const Vertex<V> &_vertex) const
332 {
333 return this->AdjacentsFrom(_vertex.Id());
334 }
335
351 public: VertexRef_M<V> AdjacentsTo(const VertexId &_vertex) const
352 {
353 auto incidentEdges = this->IncidentsTo(_vertex);
354
355 VertexRef_M<V> res;
356 for (auto const &incidentEdgeRef : incidentEdges)
357 {
358 const auto &incidentEdgeId = incidentEdgeRef.first;
359 const auto &incidentEdge = this->EdgeFromId(incidentEdgeId);
360 const auto &neighborVertexId = incidentEdge.To(_vertex);
361 const auto &neighborVertex = this->VertexFromId(neighborVertexId);
362 res.emplace(
363 std::make_pair(neighborVertexId, std::cref(neighborVertex)));
364 }
365
366 return res;
367 }
368
384 public: VertexRef_M<V> AdjacentsTo(const Vertex<V> &_vertex) const
385 {
386 return this->AdjacentsTo(_vertex.Id());
387 }
388
392 public: size_t InDegree(const VertexId &_vertex) const
393 {
394 return this->IncidentsTo(_vertex).size();
395 }
396
400 public: size_t InDegree(const Vertex<V> &_vertex) const
401 {
402 return this->IncidentsTo(this->VertexFromId(_vertex.Id())).size();
403 }
404
408 public: size_t OutDegree(const VertexId &_vertex) const
409 {
410 return this->IncidentsFrom(_vertex).size();
411 }
412
416 public: size_t OutDegree(const Vertex<V> &_vertex) const
417 {
418 return this->IncidentsFrom(this->VertexFromId(_vertex.Id())).size();
419 }
420
426 public: const EdgeRef_M<EdgeType> IncidentsFrom(const VertexId &_vertex)
427 const
428 {
430
431 const auto &adjIt = this->adjList.find(_vertex);
432 if (adjIt == this->adjList.end())
433 return res;
434
435 const auto &edgeIds = adjIt->second;
436 for (auto const &edgeId : edgeIds)
437 {
438 const auto &edge = this->EdgeFromId(edgeId);
439 if (edge.From(_vertex) != kNullId)
440 res.emplace(std::make_pair(edge.Id(), std::cref(edge)));
441 }
442
443 return res;
444 }
445
452 const Vertex<V> &_vertex) const
453 {
454 return this->IncidentsFrom(_vertex.Id());
455 }
456
463 const VertexId &_vertex) const
464 {
466
467 const auto &adjIt = this->adjList.find(_vertex);
468 if (adjIt == this->adjList.end())
469 return res;
470
471 const auto &edgeIds = adjIt->second;
472 for (auto const &edgeId : edgeIds)
473 {
474 const auto &edge = this->EdgeFromId(edgeId);
475 if (edge.To(_vertex) != kNullId)
476 res.emplace(std::make_pair(edge.Id(), std::cref(edge)));
477 }
478
479 return res;
480 }
481
487 public: const EdgeRef_M<EdgeType> IncidentsTo(const Vertex<V> &_vertex)
488 const
489 {
490 return this->IncidentsTo(_vertex.Id());
491 }
492
496 public: bool Empty() const
497 {
498 return this->vertices.empty();
499 }
500
504 public: bool RemoveVertex(const VertexId &_vertex)
505 {
506 auto vIt = this->vertices.find(_vertex);
507 if (vIt == this->vertices.end())
508 return false;
509
510 std::string name = vIt->second.Name();
511
512 // Remove incident edges.
513 auto incidents = this->IncidentsTo(_vertex);
514 for (auto edgePair : incidents)
515 this->RemoveEdge(edgePair.first);
516
517 // Remove all outgoing edges.
518 incidents = this->IncidentsFrom(_vertex);
519 for (auto edgePair : incidents)
520 this->RemoveEdge(edgePair.first);
521
522 // Remove the vertex (key) from the adjacency list.
523 this->adjList.erase(_vertex);
524
525 // Remove the vertex.
526 this->vertices.erase(_vertex);
527
528 // Get an iterator to all vertices sharing name.
529 auto iterPair = this->names.equal_range(name);
530 for (auto it = iterPair.first; it != iterPair.second; ++it)
531 {
532 if (it->second == _vertex)
533 {
534 this->names.erase(it);
535 break;
536 }
537 }
538
539 return true;
540 }
541
545 public: bool RemoveVertex(Vertex<V> &_vertex)
546 {
547 return this->RemoveVertex(_vertex.Id());
548 }
549
553 public: size_t RemoveVertices(const std::string &_name)
554 {
555 size_t numVertices = this->names.count(_name);
556 size_t result = 0;
557 for (size_t i = 0; i < numVertices; ++i)
558 {
559 auto iter = this->names.find(_name);
560 if (this->RemoveVertex(iter->second))
561 ++result;
562 }
563
564 return result;
565 }
566
572 public: bool RemoveEdge(const EdgeId &_edge)
573 {
574 auto edgeIt = this->edges.find(_edge);
575 if (edgeIt == this->edges.end())
576 return false;
577
578 auto edgeVertices = edgeIt->second.Vertices();
579
580 // Unlink the edge.
581 for (auto const &v : {edgeVertices.first, edgeVertices.second})
582 {
583 if (edgeIt->second.From(v) != kNullId)
584 {
585 auto vertex = this->adjList.find(v);
586 assert(vertex != this->adjList.end());
587 vertex->second.erase(_edge);
588 }
589 }
590
591 this->edges.erase(_edge);
592
593 return true;
594 }
595
601 public: bool RemoveEdge(EdgeType &_edge)
602 {
603 return this->RemoveEdge(_edge.Id());
604 }
605
610 public: const Vertex<V> &VertexFromId(const VertexId &_id) const
611 {
612 auto iter = this->vertices.find(_id);
613 if (iter == this->vertices.end())
615
616 return iter->second;
617 }
618
623 public: Vertex<V> &VertexFromId(const VertexId &_id)
624 {
625 auto iter = this->vertices.find(_id);
626 if (iter == this->vertices.end())
628
629 return iter->second;
630 }
631
640 public: const EdgeType &EdgeFromVertices(
641 const VertexId _sourceId, const VertexId _destId) const
642 {
643 // Get the adjacency iterator for the source vertex.
644 const typename std::map<VertexId, EdgeId_S>::const_iterator &adjIt =
645 this->adjList.find(_sourceId);
646
647 // Quit early if there is no adjacency entry
648 if (adjIt == this->adjList.end())
649 return EdgeType::NullEdge;
650
651 // Loop over the edges in the source vertex's adjacency list
652 for (std::set<EdgeId>::const_iterator edgIt = adjIt->second.begin();
653 edgIt != adjIt->second.end(); ++edgIt)
654 {
655 // Get an iterator to the actual edge
656 const typename std::map<EdgeId, EdgeType>::const_iterator edgeIter =
657 this->edges.find(*edgIt);
658
659 // Check if the edge has the correct source and destination.
660 if (edgeIter != this->edges.end() &&
661 edgeIter->second.From(_sourceId) == _destId)
662 {
663 assert(edgeIter->second.To(_destId) == _sourceId);
664 return edgeIter->second;
665 }
666 }
667
668 return EdgeType::NullEdge;
669 }
670
675 public: const EdgeType &EdgeFromId(const EdgeId &_id) const
676 {
677 auto iter = this->edges.find(_id);
678 if (iter == this->edges.end())
679 return EdgeType::NullEdge;
680
681 return iter->second;
682 }
683
689 public: template<typename VV, typename EE, typename EEdgeType>
691 const Graph<VV, EE, EEdgeType> &_g);
692
695 private: VertexId &NextVertexId()
696 {
697 while (this->vertices.find(this->nextVertexId) != this->vertices.end()
698 && this->nextVertexId < MAX_UI64)
699 {
700 ++this->nextVertexId;
701 }
702
703 return this->nextVertexId;
704 }
705
708 private: VertexId &NextEdgeId()
709 {
710 while (this->edges.find(this->nextEdgeId) != this->edges.end() &&
711 this->nextEdgeId < MAX_UI64)
712 {
713 ++this->nextEdgeId;
714 }
715
716 return this->nextEdgeId;
717 }
718
720 protected: VertexId nextVertexId = 0u;
721
723 protected: VertexId nextEdgeId = 0u;
724
726 private: std::map<VertexId, Vertex<V>> vertices;
727
729 private: std::map<EdgeId, EdgeType> edges;
730
736 private: std::map<VertexId, EdgeId_S> adjList;
737
740 };
741
744 template<typename VV, typename EE>
746 const Graph<VV, EE, UndirectedEdge<EE>> &_g)
747 {
748 _out << "graph {" << std::endl;
749
750 // All vertices with the name and Id as a "label" attribute.
751 for (auto const &vertexMap : _g.Vertices())
752 {
753 auto vertex = vertexMap.second.get();
754 _out << vertex;
755 }
756
757 // All edges.
758 for (auto const &edgeMap : _g.Edges())
759 {
760 auto edge = edgeMap.second.get();
761 _out << edge;
762 }
763
764 _out << "}" << std::endl;
765
766 return _out;
767 }
768
771 template<typename VV, typename EE>
773 const Graph<VV, EE, DirectedEdge<EE>> &_g)
774 {
775 _out << "digraph {" << std::endl;
776
777 // All vertices with the name and Id as a "label" attribute.
778 for (auto const &vertexMap : _g.Vertices())
779 {
780 auto vertex = vertexMap.second.get();
781 _out << vertex;
782 }
783
784 // All edges.
785 for (auto const &edgeMap : _g.Edges())
786 {
787 auto edge = edgeMap.second.get();
788 _out << edge;
789 }
790
791 _out << "}" << std::endl;
792
793 return _out;
794 }
795
798 template<typename V, typename E>
800
803 template<typename V, typename E>
805}
806}
807}
808}
809#endif