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 IGNITION_MATH_GRAPH_GRAPHALGORITHMS_HH_
#define IGNITION_MATH_GRAPH_GRAPHALGORITHMS_HH_

#include <functional>
#include <list>
#include <map>
#include <queue>
#include <stack>
#include <utility>
#include <vector>

#include <ignition/math/config.hh>
#include "ignition/math/graph/Graph.hh"
#include "ignition/math/Helpers.hh"

namespace ignition
{
namespace math
{
// Inline bracket to help doxygen filtering.
inline namespace IGNITION_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)
  {
    // Create an auxiliary graph, where the data is just a boolean value that
    // stores whether the vertex has been visited or not.
    Graph<bool, E, EdgeType> visitorGraph;

    // Copy the vertices (just the Id).
    for (auto const &v : _graph.Vertices())
      visitorGraph.AddVertex("", false, v.first);

    // Copy the edges (without data).
    for (auto const &e : _graph.Edges())
      visitorGraph.AddEdge(e.second.get().Vertices(), E());

    std::vector<VertexId> visited;
    std::list<VertexId> pending = {_from};

    while (!pending.empty())
    {
      auto vId = pending.front();
      pending.pop_front();

      // If the vertex has been visited, skip.
      auto &vertex = visitorGraph.VertexFromId(vId);
      if (vertex.Data())
        continue;

      visited.push_back(vId);
      vertex.Data() = true;

      // Add more vertices to visit if they haven't been visited yet.
      auto adjacents = visitorGraph.AdjacentsFrom(vId);
      for (auto const &adj : adjacents)
      {
        vId = adj.first;
        auto &v = adj.second.get();
        if (!v.Data())
          pending.push_back(vId);
      }
    }

    return visited;
  }

  template<typename V, typename E, typename EdgeType>
  std::vector<VertexId> DepthFirstSort(const Graph<V, E, EdgeType> &_graph,
                                       const VertexId &_from)
  {
    // Create an auxiliary graph, where the data is just a boolean value that
    // stores whether the vertex has been visited or not.
    Graph<bool, E, EdgeType> visitorGraph;

    // Copy the vertices (just the Id).
    for (auto const &v : _graph.Vertices())
      visitorGraph.AddVertex("", false, v.first);

    // Copy the edges (without data).
    for (auto const &e : _graph.Edges())
      visitorGraph.AddEdge(e.second.get().Vertices(), E());

    std::vector<VertexId> visited;
    std::stack<VertexId> pending({_from});

    while (!pending.empty())
    {
      auto vId = pending.top();
      pending.pop();

      // If the vertex has been visited, skip.
      auto &vertex = visitorGraph.VertexFromId(vId);
      if (vertex.Data())
        continue;

      visited.push_back(vId);
      vertex.Data() = true;

      // Add more vertices to visit if they haven't been visited yet.
      auto adjacents = visitorGraph.AdjacentsFrom(vId);
      for (auto const &adj : adjacents)
      {
        vId = adj.first;
        auto &v = adj.second.get();
        if (!v.Data())
          pending.push(vId);
      }
    }

    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::cerr << "Vertex [" << _from << "] Not found" << std::endl;
      return {};
    }

    // Sanity check: The destination vertex should exist (if used).
    if (_to != kNullId &&
        allVertices.find(_to) == allVertices.end())
    {
      std::cerr << "Vertex [" << _from << "] Not found" << std::endl;
      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 neightbor 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.
      VertexId u = pq.top().second;

      // Shortcut: Destination vertex found, exiting.
      if (_to != kNullId && _to == u)
        break;

      pq.pop();

      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 shorted path to v through u.
        if (dist[v].first > dist[u].first + weight)
        {
          // Updating 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;
  }
}
}
}
}
#endif