Graph.hh

Go to the documentation of this file.

/*
 * Copyright (C) 2017 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
#ifndef GZ_MATH_GRAPH_GRAPH_HH_
#define GZ_MATH_GRAPH_GRAPH_HH_
 
#include <cassert>
#include <iterator>
#include <map>
#include <ostream>
#include <set>
#include <sstream>
#include <string>
#include <utility>
#include <vector>
 
#include <gz/math/config.hh>
#include "gz/math/detail/Error.hh"
#include "gz/math/graph/Edge.hh"
#include "gz/math/graph/Vertex.hh"
 
namespace gz::math
{
// Inline bracket to help doxygen filtering.
inline namespace GZ_MATH_VERSION_NAMESPACE {
namespace graph
{
  //
  template<typename V, typename E, typename EdgeType>
  class Graph
  {
    public: Graph() = default;
 
    public: Graph(const std::vector<Vertex<V>> &_vertices,
                  const std::vector<EdgeInitializer<E>> &_edges)
    {
      // Add all vertices.
      for (auto const &v : _vertices)
      {
        if (!this->AddVertex(v.Name(), v.Data(), v.Id()).Valid())
        {
          std::ostringstream errStream;
          errStream << "Invalid vertex with Id [" << v.Id() << "]. Ignoring.";
          detail::LogErrorMessage(errStream.str());
        }
      }
 
      // Add all edges.
      for (auto const &e : _edges)
      {
        if (!this->AddEdge(e.vertices, e.data, e.weight).Valid())
          detail::LogErrorMessage("Ignoring edge");
      }
    }
 
    public: Vertex<V> &AddVertex(const std::string &_name,
                                 const V &_data,
                                 const VertexId &_id = kNullId)
    {
      auto id = _id;
 
      // The user didn't provide an Id, we generate it.
      if (id == kNullId)
      {
        id = this->NextVertexId();
 
        // No space for new Ids.
        if (id == kNullId)
        {
          detail::LogErrorMessage(
              "[Graph::AddVertex()] The limit of vertices has been reached. "
              "Ignoring vertex.");
          return NullVertex<V>();
        }
      }
 
      // Create the vertex.
      auto ret = this->vertices.insert(
        std::make_pair(id, Vertex<V>(_name, _data, id)));
 
      // The Id already exists.
      if (!ret.second)
      {
        std::ostringstream errStream;
        errStream << "[Graph::AddVertex()] Repeated vertex [" << id << "]";
        detail::LogErrorMessage(errStream.str());
        return NullVertex<V>();
      }
 
      // Link the vertex with an empty list of edges.
      this->adjList[id] = EdgeId_S();
 
      return ret.first->second;
    }
 
    public: const VertexRef_M<V> Vertices() const
    {
      VertexRef_M<V> res;
      for (auto const &v : this->vertices)
        res.emplace(std::make_pair(v.first, std::cref(v.second)));
 
      return res;
    }
 
    public: const VertexRef_M<V> Vertices(const std::string &_name) const
    {
      VertexRef_M<V> res;
      for (auto const &vertex : this->vertices)
      {
        if (vertex.second.Name() == _name)
          res.emplace(std::make_pair(vertex.first, std::cref(vertex.second)));
      }
 
      return res;
    }
 
    public: EdgeType &AddEdge(const VertexId_P &_vertices,
                              const E &_data,
                              const double _weight = 1.0)
    {
      auto id = this->NextEdgeId();
 
      // No space for new Ids.
      if (id == kNullId)
      {
        detail::LogErrorMessage(
            "[Graph::AddEdge()] The limit of edges has been reached. "
            "Ignoring edge.");
        return NullEdge<E, EdgeType>();
      }
 
      EdgeType newEdge(_vertices, _data, _weight, id);
      return this->LinkEdge(std::move(newEdge));
    }
 
    public: EdgeType &LinkEdge(const EdgeType &_edge)
    {
      auto edgeVertices = _edge.Vertices();
 
      // Sanity check: Both vertices should exist.
      for (auto const &v : {edgeVertices.first, edgeVertices.second})
      {
        if (this->vertices.find(v) == this->vertices.end())
          return NullEdge<E, EdgeType>();
      }
 
      // Link the new edge.
      for (auto const &v : {edgeVertices.first, edgeVertices.second})
      {
        if (v != kNullId)
        {
          auto vertexIt = this->adjList.find(v);
          assert(vertexIt != this->adjList.end());
          vertexIt->second.insert(_edge.Id());
        }
      }
 
      auto ret = this->edges.insert(std::make_pair(_edge.Id(), _edge));
 
      // Return the new edge.
      return ret.first->second;
    }
 
    public: const EdgeRef_M<EdgeType> Edges() const
    {
      EdgeRef_M<EdgeType> res;
      for (auto const &edge : this->edges)
      {
        res.emplace(std::make_pair(edge.first, std::cref(edge.second)));
      }
 
      return res;
    }
 
    public: VertexRef_M<V> AdjacentsFrom(const VertexId &_vertex) const
    {
      VertexRef_M<V> res;
 
      // Make sure the vertex exists
      auto vertexIt = this->adjList.find(_vertex);
      if (vertexIt == this->adjList.end())
        return res;
 
      for (auto const &edgeId : vertexIt->second)
      {
        const auto &edge = this->EdgeFromId(edgeId);
        auto neighborVertexId = edge.From(_vertex);
        if (neighborVertexId != kNullId)
        {
          const auto &neighborVertex = this->VertexFromId(neighborVertexId);
          res.emplace(
            std::make_pair(neighborVertexId, std::cref(neighborVertex)));
        }
      }
 
      return res;
    }
 
    public: VertexRef_M<V> AdjacentsFrom(const Vertex<V> &_vertex) const
    {
      return this->AdjacentsFrom(_vertex.Id());
    }
 
    public: VertexRef_M<V> AdjacentsTo(const VertexId &_vertex) const
    {
      auto incidentEdges = this->IncidentsTo(_vertex);
 
      VertexRef_M<V> res;
      for (auto const &incidentEdgeRef : incidentEdges)
      {
        const auto &incidentEdgeId = incidentEdgeRef.first;
        const auto &incidentEdge = this->EdgeFromId(incidentEdgeId);
        const auto &neighborVertexId = incidentEdge.To(_vertex);
        const auto &neighborVertex = this->VertexFromId(neighborVertexId);
        res.emplace(
            std::make_pair(neighborVertexId, std::cref(neighborVertex)));
      }
 
      return res;
    }
 
    public: VertexRef_M<V> AdjacentsTo(const Vertex<V> &_vertex) const
    {
      return this->AdjacentsTo(_vertex.Id());
    }
 
    public: size_t InDegree(const VertexId &_vertex) const
    {
      return this->IncidentsTo(_vertex).size();
    }
 
    public: size_t InDegree(const Vertex<V> &_vertex) const
    {
      return this->IncidentsTo(this->VertexFromId(_vertex.Id())).size();
    }
 
    public: size_t OutDegree(const VertexId &_vertex) const
    {
      return this->IncidentsFrom(_vertex).size();
    }
 
    public: size_t OutDegree(const Vertex<V> &_vertex) const
    {
      return this->IncidentsFrom(this->VertexFromId(_vertex.Id())).size();
    }
 
    public: const EdgeRef_M<EdgeType> IncidentsFrom(const VertexId &_vertex)
      const
    {
      EdgeRef_M<EdgeType> res;
 
      const auto &adjIt = this->adjList.find(_vertex);
      if (adjIt == this->adjList.end())
        return res;
 
      const auto &edgeIds = adjIt->second;
      for (auto const &edgeId : edgeIds)
      {
        const auto &edge = this->EdgeFromId(edgeId);
        if (edge.From(_vertex) != kNullId)
          res.emplace(std::make_pair(edge.Id(), std::cref(edge)));
      }
 
      return res;
    }
 
    public: const EdgeRef_M<EdgeType> IncidentsFrom(
                const Vertex<V> &_vertex) const
    {
      return this->IncidentsFrom(_vertex.Id());
    }
 
    public: const EdgeRef_M<EdgeType> IncidentsTo(
                const VertexId &_vertex) const
    {
      EdgeRef_M<EdgeType> res;
 
      const auto &adjIt = this->adjList.find(_vertex);
      if (adjIt == this->adjList.end())
        return res;
 
      const auto &edgeIds = adjIt->second;
      for (auto const &edgeId : edgeIds)
      {
        const auto &edge = this->EdgeFromId(edgeId);
        if (edge.To(_vertex) != kNullId)
          res.emplace(std::make_pair(edge.Id(), std::cref(edge)));
      }
 
      return res;
    }
 
    public: const EdgeRef_M<EdgeType> IncidentsTo(const Vertex<V> &_vertex)
      const
    {
      return this->IncidentsTo(_vertex.Id());
    }
 
    public: bool Empty() const
    {
      return this->vertices.empty();
    }
 
    public: bool RemoveVertex(const VertexId &_vertex)
    {
      auto vIt = this->vertices.find(_vertex);
      if (vIt == this->vertices.end())
        return false;
 
      // Remove incident edges.
      auto incidents = this->IncidentsTo(_vertex);
      for (auto edgePair : incidents)
        this->RemoveEdge(edgePair.first);
 
      // Remove all outgoing edges.
      incidents = this->IncidentsFrom(_vertex);
      for (auto edgePair : incidents)
        this->RemoveEdge(edgePair.first);
 
      // Remove the vertex (key) from the adjacency list.
      this->adjList.erase(_vertex);
 
      // Remove the vertex.
      this->vertices.erase(_vertex);
 
      return true;
    }
 
    public: bool RemoveVertex(Vertex<V> &_vertex)
    {
      return this->RemoveVertex(_vertex.Id());
    }
 
    public: size_t RemoveVertices(const std::string &_name)
    {
      std::vector<VertexId> toRemove;
      for (auto const &[id, vertex] : this->vertices)
      {
        if (vertex.Name() == _name)
          toRemove.push_back(id);
      }
 
      size_t result = 0;
      for (auto const &id : toRemove)
      {
        if (this->RemoveVertex(id))
          ++result;
      }
      return result;
    }
 
    public: bool RemoveEdge(const EdgeId &_edge)
    {
      auto edgeIt = this->edges.find(_edge);
      if (edgeIt == this->edges.end())
        return false;
 
      auto edgeVertices = edgeIt->second.Vertices();
 
      // Unlink the edge.
      for (auto const &v : {edgeVertices.first, edgeVertices.second})
      {
        if (edgeIt->second.From(v) != kNullId)
        {
          auto vertex = this->adjList.find(v);
          assert(vertex != this->adjList.end());
          vertex->second.erase(_edge);
        }
      }
 
      this->edges.erase(_edge);
 
      return true;
    }
 
    public: bool RemoveEdge(EdgeType &_edge)
    {
      return this->RemoveEdge(_edge.Id());
    }
 
    public: const Vertex<V> &VertexFromId(const VertexId &_id) const
    {
      auto iter = this->vertices.find(_id);
      if (iter == this->vertices.end())
        return NullVertex<V>();
 
      return iter->second;
    }
 
    public: Vertex<V> &VertexFromId(const VertexId &_id)
    {
      auto iter = this->vertices.find(_id);
      if (iter == this->vertices.end())
        return NullVertex<V>();
 
      return iter->second;
    }
 
    public: const EdgeType &EdgeFromVertices(
                const VertexId _sourceId, const VertexId _destId) const
    {
      // Get the adjacency iterator for the source vertex.
      const typename std::map<VertexId, EdgeId_S>::const_iterator &adjIt =
        this->adjList.find(_sourceId);
 
      // Quit early if there is no adjacency entry
      if (adjIt == this->adjList.end())
        return NullEdge<E, EdgeType>();
 
      // Loop over the edges in the source vertex's adjacency list
      for (std::set<EdgeId>::const_iterator edgIt = adjIt->second.begin();
           edgIt != adjIt->second.end(); ++edgIt)
      {
        // Get an iterator to the actual edge
        const typename std::map<EdgeId, EdgeType>::const_iterator edgeIter =
          this->edges.find(*edgIt);
 
        // Check if the edge has the correct source and destination.
        if (edgeIter != this->edges.end() &&
            edgeIter->second.From(_sourceId) == _destId)
        {
          assert(edgeIter->second.To(_destId) == _sourceId);
          return edgeIter->second;
        }
      }
 
      return NullEdge<E, EdgeType>();
    }
 
    public: const EdgeType &EdgeFromId(const EdgeId &_id) const
    {
      auto iter = this->edges.find(_id);
      if (iter == this->edges.end())
        return NullEdge<E, EdgeType>();
 
      return iter->second;
    }
 
    public: EdgeType &EdgeFromId(const EdgeId &_id)
    {
      auto iter = this->edges.find(_id);
      if (iter == this->edges.end())
        return NullEdge<E, EdgeType>();
 
      return iter->second;
    }
 
    public: template<typename VV, typename EE, typename EEdgeType>
    friend std::ostream &operator<<(std::ostream &_out,
                                    const Graph<VV, EE, EEdgeType> &_g);
 
    private: VertexId &NextVertexId()
    {
      while (this->vertices.find(this->nextVertexId) != this->vertices.end()
          && this->nextVertexId < MAX_UI64)
      {
        ++this->nextVertexId;
      }
 
      return this->nextVertexId;
    }
 
    private: VertexId &NextEdgeId()
    {
      while (this->edges.find(this->nextEdgeId) != this->edges.end() &&
             this->nextEdgeId < MAX_UI64)
      {
        ++this->nextEdgeId;
      }
 
      return this->nextEdgeId;
    }
 
    protected: VertexId nextVertexId = 0u;
 
    protected: VertexId nextEdgeId = 0u;
 
    private: std::map<VertexId, Vertex<V>> vertices;
 
    private: std::map<EdgeId, EdgeType> edges;
 
    private: std::map<VertexId, EdgeId_S> adjList;
 
    private: std::multimap<std::string, VertexId> names;
  };
 
  template<typename VV, typename EE>
  std::ostream &operator<<(std::ostream &_out,
                           const Graph<VV, EE, UndirectedEdge<EE>> &_g)
  {
    _out << "graph {" << std::endl;
 
    // All vertices with the name and Id as a "label" attribute.
    for (auto const &vertexMap : _g.Vertices())
    {
      auto vertex = vertexMap.second.get();
      _out << vertex;
    }
 
    // All edges.
    for (auto const &edgeMap : _g.Edges())
    {
      auto edge = edgeMap.second.get();
      _out << edge;
    }
 
    _out << "}" << std::endl;
 
    return _out;
  }
 
  template<typename VV, typename EE>
  std::ostream &operator<<(std::ostream &_out,
                           const Graph<VV, EE, DirectedEdge<EE>> &_g)
  {
    _out << "digraph {" << std::endl;
 
    // All vertices with the name and Id as a "label" attribute.
    for (auto const &vertexMap : _g.Vertices())
    {
      auto vertex = vertexMap.second.get();
      _out << vertex;
    }
 
    // All edges.
    for (auto const &edgeMap : _g.Edges())
    {
      auto edge = edgeMap.second.get();
      _out << edge;
    }
 
    _out << "}" << std::endl;
 
    return _out;
  }
 
  template<typename V, typename E>
  using UndirectedGraph = Graph<V, E, UndirectedEdge<E>>;
 
  template<typename V, typename E>
  using DirectedGraph = Graph<V, E, DirectedEdge<E>>;
}  // namespace graph
}  // namespace GZ_MATH_VERSION_NAMESPACE
}  // namespace gz::math::graph
#endif  // GZ_MATH_GRAPH_GRAPH_HH_