Loading...
Searching...
No Matches
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 IGNITION_MATH_GRAPH_GRAPH_HH_
18#define IGNITION_MATH_GRAPH_GRAPH_HH_
19
20#include <cassert>
21#include <iostream>
22#include <map>
23#include <string>
24#include <vector>
25
26#include <ignition/math/config.hh>
29
30namespace ignition
31{
32namespace math
33{
34inline namespace IGNITION_MATH_VERSION_NAMESPACE
35{
36namespace 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;
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;
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 std::move(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 std::move(res);
201 }
202
208 public: EdgeType &AddEdge(const VertexId_P &_vertices,
209 const E &_data,
210 const double _weight = 1.0)
211 {
212 auto id = this->NextEdgeId();
213
214 // No space for new Ids.
215 if (id == kNullId)
216 {
217 std::cerr << "[Graph::AddEdge()] The limit of edges has been reached. "
218 << "Ignoring edge." << std::endl;
219 return EdgeType::NullEdge;
220 }
221
222 EdgeType newEdge(_vertices, _data, _weight, id);
223 return this->LinkEdge(std::move(newEdge));
224 }
225
232 public: EdgeType &LinkEdge(const EdgeType &_edge)
233 {
234 auto edgeVertices = _edge.Vertices();
235
236 // Sanity check: Both vertices should exist.
237 for (auto const &v : {edgeVertices.first, edgeVertices.second})
238 {
239 if (this->vertices.find(v) == this->vertices.end())
240 return EdgeType::NullEdge;
241 }
242
243 // Link the new edge.
244 for (auto const &v : {edgeVertices.first, edgeVertices.second})
245 {
246 if (v != kNullId)
247 {
248 auto vertexIt = this->adjList.find(v);
249 assert(vertexIt != this->adjList.end());
250 vertexIt->second.insert(_edge.Id());
251 }
252 }
253
254 auto ret = this->edges.insert(std::make_pair(_edge.Id(), _edge));
255
256 // Return the new edge.
257 return ret.first->second;
258 }
259
263 public: const EdgeRef_M<EdgeType> Edges() const
264 {
266 for (auto const &edge : this->edges)
267 {
268 res.emplace(std::make_pair(edge.first, std::cref(edge.second)));
269 }
270
271 return std::move(res);
272 }
273
289 public: VertexRef_M<V> AdjacentsFrom(const VertexId &_vertex) const
290 {
291 VertexRef_M<V> res;
292
293 // Make sure the vertex exists
294 auto vertexIt = this->adjList.find(_vertex);
295 if (vertexIt == this->adjList.end())
296 return res;
297
298 for (auto const &edgeId : vertexIt->second)
299 {
300 const auto &edge = this->EdgeFromId(edgeId);
301 auto neighborVertexId = edge.From(_vertex);
302 if (neighborVertexId != kNullId)
303 {
304 const auto &neighborVertex = this->VertexFromId(neighborVertexId);
305 res.emplace(
306 std::make_pair(neighborVertexId, std::cref(neighborVertex)));
307 }
308 }
309
310 return res;
311 }
312
328 public: VertexRef_M<V> AdjacentsFrom(const Vertex<V> &_vertex) const
329 {
330 return this->AdjacentsFrom(_vertex.Id());
331 }
332
348 public: VertexRef_M<V> AdjacentsTo(const VertexId &_vertex) const
349 {
350 auto incidentEdges = this->IncidentsTo(_vertex);
351
352 VertexRef_M<V> res;
353 for (auto const &incidentEdgeRef : incidentEdges)
354 {
355 const auto &incidentEdgeId = incidentEdgeRef.first;
356 const auto &incidentEdge = this->EdgeFromId(incidentEdgeId);
357 const auto &neighborVertexId = incidentEdge.To(_vertex);
358 const auto &neighborVertex = this->VertexFromId(neighborVertexId);
359 res.emplace(
360 std::make_pair(neighborVertexId, std::cref(neighborVertex)));
361 }
362
363 return res;
364 }
365
381 public: VertexRef_M<V> AdjacentsTo(const Vertex<V> &_vertex) const
382 {
383 return this->AdjacentsTo(_vertex.Id());
384 }
385
389 public: size_t InDegree(const VertexId &_vertex) const
390 {
391 return this->IncidentsTo(_vertex).size();
392 }
393
397 public: size_t InDegree(const Vertex<V> &_vertex) const
398 {
399 return this->IncidentsTo(this->VertexFromId(_vertex.Id())).size();
400 }
401
405 public: size_t OutDegree(const VertexId &_vertex) const
406 {
407 return this->IncidentsFrom(_vertex).size();
408 }
409
413 public: size_t OutDegree(const Vertex<V> &_vertex) const
414 {
415 return this->IncidentsFrom(this->VertexFromId(_vertex.Id())).size();
416 }
417
423 public: const EdgeRef_M<EdgeType> IncidentsFrom(const VertexId &_vertex)
424 const
425 {
427
428 const auto &adjIt = this->adjList.find(_vertex);
429 if (adjIt == this->adjList.end())
430 return res;
431
432 const auto &edgeIds = adjIt->second;
433 for (auto const &edgeId : edgeIds)
434 {
435 const auto &edge = this->EdgeFromId(edgeId);
436 if (edge.From(_vertex) != kNullId)
437 res.emplace(std::make_pair(edge.Id(), std::cref(edge)));
438 }
439
440 return std::move(res);
441 }
442
449 const Vertex<V> &_vertex) const
450 {
451 return this->IncidentsFrom(_vertex.Id());
452 }
453
460 const VertexId &_vertex) const
461 {
463
464 const auto &adjIt = this->adjList.find(_vertex);
465 if (adjIt == this->adjList.end())
466 return res;
467
468 const auto &edgeIds = adjIt->second;
469 for (auto const &edgeId : edgeIds)
470 {
471 const auto &edge = this->EdgeFromId(edgeId);
472 if (edge.To(_vertex) != kNullId)
473 res.emplace(std::make_pair(edge.Id(), std::cref(edge)));
474 }
475
476 return std::move(res);
477 }
478
484 public: const EdgeRef_M<EdgeType> IncidentsTo(const Vertex<V> &_vertex)
485 const
486 {
487 return this->IncidentsTo(_vertex.Id());
488 }
489
493 public: bool Empty() const
494 {
495 return this->vertices.empty();
496 }
497
501 public: bool RemoveVertex(const VertexId &_vertex)
502 {
503 auto vIt = this->vertices.find(_vertex);
504 if (vIt == this->vertices.end())
505 return false;
506
507 std::string name = vIt->second.Name();
508
509 // Remove incident edges.
510 auto incidents = this->IncidentsTo(_vertex);
511 for (auto edgePair : incidents)
512 this->RemoveEdge(edgePair.first);
513
514 // Remove all outgoing edges.
515 incidents = this->IncidentsFrom(_vertex);
516 for (auto edgePair : incidents)
517 this->RemoveEdge(edgePair.first);
518
519 // Remove the vertex (key) from the adjacency list.
520 this->adjList.erase(_vertex);
521
522 // Remove the vertex.
523 this->vertices.erase(_vertex);
524
525 // Get an iterator to all vertices sharing name.
526 auto iterPair = this->names.equal_range(name);
527 for (auto it = iterPair.first; it != iterPair.second; ++it)
528 {
529 if (it->second == _vertex)
530 {
531 this->names.erase(it);
532 break;
533 }
534 }
535
536 return true;
537 }
538
542 public: bool RemoveVertex(Vertex<V> &_vertex)
543 {
544 return this->RemoveVertex(_vertex.Id());
545 }
546
550 public: size_t RemoveVertices(const std::string &_name)
551 {
552 size_t numVertices = this->names.count(_name);
553 size_t result = 0;
554 for (size_t i = 0; i < numVertices; ++i)
555 {
556 auto iter = this->names.find(_name);
557 if (this->RemoveVertex(iter->second))
558 ++result;
559 }
560
561 return result;
562 }
563
569 public: bool RemoveEdge(const EdgeId &_edge)
570 {
571 auto edgeIt = this->edges.find(_edge);
572 if (edgeIt == this->edges.end())
573 return false;
574
575 auto edgeVertices = edgeIt->second.Vertices();
576
577 // Unlink the edge.
578 for (auto const &v : {edgeVertices.first, edgeVertices.second})
579 {
580 if (edgeIt->second.From(v) != kNullId)
581 {
582 auto vertex = this->adjList.find(v);
583 assert(vertex != this->adjList.end());
584 vertex->second.erase(_edge);
585 }
586 }
587
588 this->edges.erase(_edge);
589
590 return true;
591 }
592
598 public: bool RemoveEdge(EdgeType &_edge)
599 {
600 return this->RemoveEdge(_edge.Id());
601 }
602
607 public: const Vertex<V> &VertexFromId(const VertexId &_id) const
608 {
609 auto iter = this->vertices.find(_id);
610 if (iter == this->vertices.end())
612
613 return iter->second;
614 }
615
620 public: Vertex<V> &VertexFromId(const VertexId &_id)
621 {
622 auto iter = this->vertices.find(_id);
623 if (iter == this->vertices.end())
625
626 return iter->second;
627 }
628
633 public: const EdgeType &EdgeFromId(const EdgeId &_id) const
634 {
635 auto iter = this->edges.find(_id);
636 if (iter == this->edges.end())
637 return EdgeType::NullEdge;
638
639 return iter->second;
640 }
641
647 public: template<typename VV, typename EE, typename EEdgeType>
648 friend std::ostream &operator<<(std::ostream &_out,
649 const Graph<VV, EE, EEdgeType> &_g);
650
653 private: VertexId &NextVertexId()
654 {
655 while (this->vertices.find(this->nextVertexId) != this->vertices.end()
656 && this->nextVertexId < MAX_UI64)
657 {
658 ++this->nextVertexId;
659 }
660
661 return this->nextVertexId;
662 }
663
666 private: VertexId &NextEdgeId()
667 {
668 while (this->edges.find(this->nextEdgeId) != this->edges.end() &&
669 this->nextEdgeId < MAX_UI64)
670 {
671 ++this->nextEdgeId;
672 }
673
674 return this->nextEdgeId;
675 }
676
678 protected: VertexId nextVertexId = 0u;
679
681 protected: VertexId nextEdgeId = 0u;
682
684 private: std::map<VertexId, Vertex<V>> vertices;
685
687 private: std::map<EdgeId, EdgeType> edges;
688
694 private: std::map<VertexId, EdgeId_S> adjList;
695
697 private: std::multimap<std::string, VertexId> names;
698 };
699
702 template<typename VV, typename EE>
703 std::ostream &operator<<(std::ostream &_out,
704 const Graph<VV, EE, UndirectedEdge<EE>> &_g)
705 {
706 _out << "graph {" << std::endl;
707
708 // All vertices with the name and Id as a "label" attribute.
709 for (auto const &vertexMap : _g.Vertices())
710 {
711 auto vertex = vertexMap.second.get();
712 _out << vertex;
713 }
714
715 // All edges.
716 for (auto const &edgeMap : _g.Edges())
717 {
718 auto edge = edgeMap.second.get();
719 _out << edge;
720 }
721
722 _out << "}" << std::endl;
723
724 return _out;
725 }
726
729 template<typename VV, typename EE>
730 std::ostream &operator<<(std::ostream &_out,
731 const Graph<VV, EE, DirectedEdge<EE>> &_g)
732 {
733 _out << "digraph {" << std::endl;
734
735 // All vertices with the name and Id as a "label" attribute.
736 for (auto const &vertexMap : _g.Vertices())
737 {
738 auto vertex = vertexMap.second.get();
739 _out << vertex;
740 }
741
742 // All edges.
743 for (auto const &edgeMap : _g.Edges())
744 {
745 auto edge = edgeMap.second.get();
746 _out << edge;
747 }
748
749 _out << "}" << std::endl;
750
751 return _out;
752 }
753
756 template<typename V, typename E>
758
761 template<typename V, typename E>
763}
764}
765}
766}
767#endif
A directed edge represents a connection between two vertices.
Definition Edge.hh:268
A generic graph class.
Definition Graph.hh:101
VertexRef_M< V > AdjacentsFrom(const Vertex< V > &_vertex) const
Get all vertices that are directly connected with one edge from a given vertex.
Definition Graph.hh:328
const EdgeRef_M< EdgeType > IncidentsFrom(const Vertex< V > &_vertex) const
Get the set of outgoing edges from a given vertex.
Definition Graph.hh:448
EdgeType & AddEdge(const VertexId_P &_vertices, const E &_data, const double _weight=1.0)
Add a new edge to the graph.
Definition Graph.hh:208
Vertex< V > & VertexFromId(const VertexId &_id)
Get a mutable reference to a vertex using its Id.
Definition Graph.hh:620
bool RemoveEdge(EdgeType &_edge)
Remove an existing edge from the graph.
Definition Graph.hh:598
const VertexRef_M< V > Vertices(const std::string &_name) const
The collection of all vertices in the graph with name == _name.
Definition Graph.hh:191
const EdgeType & EdgeFromId(const EdgeId &_id) const
Get a reference to an edge using its Id.
Definition Graph.hh:633
bool Empty() const
Get whether the graph is empty.
Definition Graph.hh:493
VertexRef_M< V > AdjacentsTo(const Vertex< V > &_vertex) const
Get all vertices that are directly connected with one edge to a given vertex.
Definition Graph.hh:381
VertexId nextEdgeId
The next edge Id to be assigned to a new edge.
Definition Graph.hh:681
const Vertex< V > & VertexFromId(const VertexId &_id) const
Get a reference to a vertex using its Id.
Definition Graph.hh:607
const EdgeRef_M< EdgeType > IncidentsTo(const Vertex< V > &_vertex) const
Get the set of incoming edges to a given vertex.
Definition Graph.hh:484
size_t InDegree(const Vertex< V > &_vertex) const
Get the number of edges incident to a vertex.
Definition Graph.hh:397
bool RemoveVertex(const VertexId &_vertex)
Remove an existing vertex from the graph.
Definition Graph.hh:501
const VertexRef_M< V > Vertices() const
The collection of all vertices in the graph.
Definition Graph.hh:179
VertexId nextVertexId
The next vertex Id to be assigned to a new vertex.
Definition Graph.hh:678
const EdgeRef_M< EdgeType > IncidentsFrom(const VertexId &_vertex) const
Get the set of outgoing edges from a given vertex.
Definition Graph.hh:423
const EdgeRef_M< EdgeType > Edges() const
The collection of all edges in the graph.
Definition Graph.hh:263
EdgeType & LinkEdge(const EdgeType &_edge)
Links an edge to the graph.
Definition Graph.hh:232
Graph(const std::vector< Vertex< V > > &_vertices, const std::vector< EdgeInitializer< E > > &_edges)
Constructor.
Definition Graph.hh:108
bool RemoveEdge(const EdgeId &_edge)
Remove an existing edge from the graph.
Definition Graph.hh:569
const EdgeRef_M< EdgeType > IncidentsTo(const VertexId &_vertex) const
Get the set of incoming edges to a given vertex.
Definition Graph.hh:459
size_t RemoveVertices(const std::string &_name)
Remove all vertices with name == _name.
Definition Graph.hh:550
size_t OutDegree(const Vertex< V > &_vertex) const
Get the number of edges incident from a vertex.
Definition Graph.hh:413
Vertex< V > & AddVertex(const std::string &_name, const V &_data, const VertexId &_id=kNullId)
Add a new vertex to the graph.
Definition Graph.hh:135
friend std::ostream & operator<<(std::ostream &_out, const Graph< VV, EE, EEdgeType > &_g)
Stream insertion operator.
VertexRef_M< V > AdjacentsTo(const VertexId &_vertex) const
Get all vertices that are directly connected with one edge to a given vertex.
Definition Graph.hh:348
bool RemoveVertex(Vertex< V > &_vertex)
Remove an existing vertex from the graph.
Definition Graph.hh:542
size_t InDegree(const VertexId &_vertex) const
Get the number of edges incident to a vertex.
Definition Graph.hh:389
size_t OutDegree(const VertexId &_vertex) const
Get the number of edges incident from a vertex.
Definition Graph.hh:405
VertexRef_M< V > AdjacentsFrom(const VertexId &_vertex) const
Get all vertices that are directly connected with one edge from a given vertex.
Definition Graph.hh:289
An undirected edge represents a connection between two vertices.
Definition Edge.hh:205
A vertex of a graph.
Definition Vertex.hh:55
VertexId Id() const
Get the vertex Id.
Definition Vertex.hh:88
std::pair< VertexId, VertexId > VertexId_P
Definition Vertex.hh:45
std::map< EdgeId, std::reference_wrapper< const EdgeType > > EdgeRef_M
Definition Edge.hh:198
std::ostream & operator<<(std::ostream &_out, const Graph< VV, EE, UndirectedEdge< EE > > &_g)
Partial template specification for undirected edges.
Definition Graph.hh:703
static const VertexId kNullId
Represents an invalid Id.
Definition Vertex.hh:48
std::set< EdgeId > EdgeId_S
Definition Edge.hh:192
std::map< VertexId, std::reference_wrapper< const Vertex< V > > > VertexRef_M
Definition Vertex.hh:138
static const uint64_t MAX_UI64
64bit unsigned integer maximum value
Definition Helpers.hh:328
Definition Angle.hh:40
Used in the Graph constructors for uniform initialization.
Definition Edge.hh:45