GraphAlgorithms.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_GRAPHALGORITHMS_HH_
#define GZ_MATH_GRAPH_GRAPHALGORITHMS_HH_
 
#include <functional>
#include <map>
#include <queue>
#include <stack>
#include <unordered_set>
#include <utility>
#include <vector>
 
#include <gz/math/config.hh>
#include "gz/math/detail/Error.hh"
#include "gz/math/graph/Graph.hh"
#include "gz/math/Helpers.hh"
 
namespace gz::math
{
// Inline bracket to help doxygen filtering.
inline namespace GZ_MATH_VERSION_NAMESPACE {
namespace graph
{
  using CostInfo = std::pair<double, VertexId>;
 
  template<typename V, typename E, typename EdgeType>
  std::vector<VertexId> BreadthFirstSort(const Graph<V, E, EdgeType> &_graph,
                                         const VertexId &_from)
  {
    if (!_graph.VertexFromId(_from).Valid())
      return {};
 
    std::vector<VertexId> visited;
    std::unordered_set<VertexId> seen;
    std::queue<VertexId> pending;
 
    // Mark-on-enqueue: each vertex enters the queue at most once.
    pending.push(_from);
    seen.insert(_from);
 
    while (!pending.empty())
    {
      const VertexId u = pending.front();
      pending.pop();
      visited.push_back(u);
 
      for (auto const &adj : _graph.AdjacentsFrom(u))
      {
        const VertexId next = adj.first;
        if (seen.insert(next).second)
          pending.push(next);
      }
    }
    return visited;
  }
 
  template<typename V, typename E, typename EdgeType>
  std::vector<VertexId> DepthFirstSort(const Graph<V, E, EdgeType> &_graph,
                                       const VertexId &_from)
  {
    if (!_graph.VertexFromId(_from).Valid())
      return {};
 
    std::vector<VertexId> visited;
    std::unordered_set<VertexId> seen;
    std::stack<VertexId> pending;
    pending.push(_from);
 
    // Mark-on-pop: matches the textbook DFS visitation order. Children are
    // pushed unconditionally and duplicate entries are skipped at pop time.
    while (!pending.empty())
    {
      const VertexId u = pending.top();
      pending.pop();
 
      if (!seen.insert(u).second)
        continue;
      visited.push_back(u);
 
      for (auto const &adj : _graph.AdjacentsFrom(u))
      {
        const VertexId next = adj.first;
        if (!seen.count(next))
          pending.push(next);
      }
    }
    return visited;
  }
 
  template<typename V, typename E, typename EdgeType>
  std::map<VertexId, CostInfo> Dijkstra(const Graph<V, E, EdgeType> &_graph,
                                        const VertexId &_from,
                                        const VertexId &_to = kNullId)
  {
    auto allVertices = _graph.Vertices();
 
    // Sanity check: The source vertex should exist.
    if (allVertices.find(_from) == allVertices.end())
    {
      std::ostringstream errStream;
      errStream << "Vertex [" << _from << "] Not found";
      detail::LogErrorMessage(errStream.str());
      return {};
    }
 
    // Sanity check: The destination vertex should exist (if used).
    if (_to != kNullId &&
        allVertices.find(_to) == allVertices.end())
    {
      std::ostringstream errStream;
      errStream << "Vertex [" << _to << "] Not found";
      detail::LogErrorMessage(errStream.str());
      return {};
    }
 
    // Store vertices that are being preprocessed.
    std::priority_queue<CostInfo,
      std::vector<CostInfo>, std::greater<CostInfo>> pq;
 
    // Create a map for distances and next neighbor and initialize all
    // distances as infinite.
    std::map<VertexId, CostInfo> dist;
    for (auto const &v : allVertices)
    {
      auto id = v.first;
      dist[id] = std::make_pair(MAX_D, kNullId);
    }
 
    // Insert _from in the priority queue and initialize its distance as 0.
    pq.push(std::make_pair(0.0, _from));
    dist[_from] = std::make_pair(0.0, _from);
 
    while (!pq.empty())
    {
      // This is the minimum distance vertex.
      const double poppedCost = pq.top().first;
      VertexId u = pq.top().second;
 
      // Shortcut: Destination vertex found, exiting.
      if (_to != kNullId && _to == u)
        break;
 
      pq.pop();
 
      // Skip stale priority-queue entries left behind by relaxation
      // updates: dist[u] is the authoritative cost; if the popped cost is
      // greater, this entry was queued before u was settled.
      if (poppedCost > dist[u].first)
        continue;
 
      for (auto const &edgePair : _graph.IncidentsFrom(u))
      {
        const auto &edge = edgePair.second.get();
        const auto &v = edge.From(u);
        double weight = edge.Weight();
 
        // If there is a shorter path to v through u.
        if (dist[v].first > dist[u].first + weight)
        {
          // Update distance of v.
          dist[v] = std::make_pair(dist[u].first + weight, u);
          pq.push(std::make_pair(dist[v].first, v));
        }
      }
    }
 
    return dist;
  }
 
  template<typename V, typename E>
  std::vector<UndirectedGraph<V, E>> ConnectedComponents(
    const UndirectedGraph<V, E> &_graph)
  {
    std::map<VertexId, unsigned int> visited;
    unsigned int componentCount = 0;
 
    for (auto const &v : _graph.Vertices())
    {
      if (visited.find(v.first) == visited.end())
      {
        auto component = BreadthFirstSort(_graph, v.first);
        for (auto const &vId : component)
          visited[vId] = componentCount;
        ++componentCount;
      }
    }
 
    std::vector<UndirectedGraph<V, E>> res(componentCount);
 
    // Create the vertices.
    for (auto const &vPair : _graph.Vertices())
    {
      const auto &v = vPair.second.get();
      const auto &componentId = visited[v.Id()];
      res[componentId].AddVertex(v.Name(), v.Data(), v.Id());
    }
 
    // Create the edges.
    for (auto const &ePair : _graph.Edges())
    {
      const auto &e = ePair.second.get();
      const auto &vertices = e.Vertices();
      const auto &componentId = visited[vertices.first];
      res[componentId].AddEdge(vertices, e.Data(), e.Weight());
    }
 
    return res;
  }
 
  template<typename V, typename E>
  UndirectedGraph<V, E> ToUndirectedGraph(const DirectedGraph<V, E> &_graph)
  {
    std::vector<Vertex<V>> vertices;
    std::vector<EdgeInitializer<E>> edges;
 
    // Add all vertices.
    for (auto const &vPair : _graph.Vertices())
    {
      // cppcheck-suppress useStlAlgorithm
      vertices.push_back(vPair.second.get());
    }
 
    // Add all edges.
    for (auto const &ePair : _graph.Edges())
    {
      auto const &e = ePair.second.get();
      edges.push_back({e.Vertices(), e.Data(), e.Weight()});
    }
 
    return UndirectedGraph<V, E>(vertices, edges);
  }
 
  template<typename V, typename E, typename EdgeType>
  std::pair<std::vector<VertexId>, bool> Ancestors(
      const Graph<V, E, EdgeType> &_graph, const VertexId &_vertex)
  {
    std::vector<VertexId> chain;
    if (!_graph.VertexFromId(_vertex).Valid())
      return {chain, false};
 
    std::unordered_set<VertexId> seen;
    seen.insert(_vertex);
    VertexId cur = _vertex;
    while (true)
    {
      auto parents = _graph.AdjacentsTo(cur);
      if (parents.empty())
        return {chain, true};
      const VertexId next = parents.begin()->first;
      // Cycle guard: stop if we revisit a vertex.
      if (!seen.insert(next).second)
        return {chain, false};
      chain.push_back(next);
      cur = next;
    }
  }
 
  template<typename V, typename E, typename EdgeType>
  bool IsAncestor(
      const Graph<V, E, EdgeType> &_graph,
      const VertexId &_ancestor,
      const VertexId &_descendant)
  {
    if (_ancestor == _descendant)
      return false;
    if (!_graph.VertexFromId(_ancestor).Valid() ||
        !_graph.VertexFromId(_descendant).Valid())
    {
      return false;
    }
 
    std::unordered_set<VertexId> seen;
    seen.insert(_descendant);
    VertexId cur = _descendant;
    while (true)
    {
      auto parents = _graph.AdjacentsTo(cur);
      if (parents.empty())
        return false;
      const VertexId next = parents.begin()->first;
      if (next == _ancestor)
        return true;
      // Cycle guard.
      if (!seen.insert(next).second)
        return false;
      cur = next;
    }
  }
 
  template<typename V, typename E, typename EdgeType>
  VertexId LowestCommonAncestor(
      const Graph<V, E, EdgeType> &_graph,
      const VertexId &_a, const VertexId &_b)
  {
    if (!_graph.VertexFromId(_a).Valid() ||
        !_graph.VertexFromId(_b).Valid())
    {
      return kNullId;
    }
    if (_a == _b)
      return _a;
 
    std::unordered_set<VertexId> ancestorsA;
    ancestorsA.insert(_a);
    for (auto v : Ancestors(_graph, _a).first)
      ancestorsA.insert(v);
 
    if (ancestorsA.count(_b))
      return _b;
    for (auto v : Ancestors(_graph, _b).first)
    {
      if (ancestorsA.count(v))
        return v;
    }
    return kNullId;
  }
 
  template<typename V, typename E, typename EdgeType>
  Graph<V, E, EdgeType> Subgraph(
      const Graph<V, E, EdgeType> &_graph, const VertexId &_root)
  {
    Graph<V, E, EdgeType> out;
    if (!_graph.VertexFromId(_root).Valid())
      return out;
 
    auto descendants = BreadthFirstSort(_graph, _root);
    std::unordered_set<VertexId> set(descendants.begin(), descendants.end());
 
    // Copy vertices preserving ids.
    for (auto id : descendants)
    {
      const auto &v = _graph.VertexFromId(id);
      out.AddVertex(v.Name(), v.Data(), v.Id());
    }
    // Copy edges whose endpoints both lie in the reachable set.
    for (auto const &ePair : _graph.Edges())
    {
      auto const &e = ePair.second.get();
      auto vs = e.Vertices();
      if (set.count(vs.first) && set.count(vs.second))
        out.AddEdge(vs, e.Data(), e.Weight());
    }
    return out;
  }
 
  template<typename V, typename E, typename EdgeType>
  std::unordered_set<VertexId> DescendantsSet(
      const Graph<V, E, EdgeType> &_graph, const VertexId &_vertex)
  {
    std::unordered_set<VertexId> out;
    if (!_graph.VertexFromId(_vertex).Valid())
      return out;
 
    std::queue<VertexId> pending;
    pending.push(_vertex);
    out.insert(_vertex);
    while (!pending.empty())
    {
      const VertexId u = pending.front();
      pending.pop();
      for (auto const &adj : _graph.AdjacentsFrom(u))
      {
        if (out.insert(adj.first).second)
          pending.push(adj.first);
      }
    }
    return out;
  }
}  // namespace graph
}  // namespace GZ_MATH_VERSION_NAMESPACE
}  // namespace gz::math
#endif   // GZ_MATH_GRAPH_GRAPHALGORITHMS_HH_