ReconstructPath.hpp

Go to the documentation of this file.

 
// Copyright (c) 2017 Czech Technical University in Prague | Faculty of Information Technology. All rights reserved.
 
#pragma once
 
#include <alib/map>
#include <alib/vector>
#include <algorithm>
 
namespace graph::common {
 
class ReconstructPath {
// ---------------------------------------------------------------------------------------------------------------------
 public:
  template<typename TNode>
  static ext::vector<TNode> reconstructPath(const ext::map<TNode, TNode> &p,
                                            const TNode &start,
                                            const TNode &goal);
 
// ---------------------------------------------------------------------------------------------------------------------
 
  template<typename TNode>
  static ext::vector<TNode> reconstructPath(const ext::map<TNode, TNode> &p_forward,
                                            const ext::map<TNode, TNode> &p_backward,
                                            const TNode &start,
                                            const TNode &goal,
                                            const TNode &intersection_node);
 
// ---------------------------------------------------------------------------------------------------------------------
 
  template<typename TNode, typename TWeight>
  static
  ext::pair<ext::vector<TNode>, TWeight>
  reconstructWeightedPath(const ext::map<TNode, TNode> &p,
                          const ext::map<TNode, TWeight> &g,
                          const TNode &start,
                          const TNode &goal);
 
// ---------------------------------------------------------------------------------------------------------------------
 
  template<typename TNode, typename TWeight>
  static
  ext::pair<ext::vector<TNode>, TWeight>
  reconstructWeightedPath(const ext::map<TNode, TNode> &p_forward,
                          const ext::map<TNode, TNode> &p_backward,
                          const ext::map<TNode, TWeight> &g_forward,
                          const ext::map<TNode, TWeight> &g_backward,
                          const TNode &start,
                          const TNode &goal,
                          const TNode &intersection_node);
 
// ---------------------------------------------------------------------------------------------------------------------
 
  template<typename TNode>
  static
  ext::vector<TNode>
  joinPath(ext::vector<TNode> &forward_path, const ext::vector<TNode> &backward_path);
 
// ---------------------------------------------------------------------------------------------------------------------
};
// =====================================================================================================================
 
template<typename TNode>
ext::vector<TNode> ReconstructPath::reconstructPath(const ext::map<TNode, TNode> &p,
                                                    const TNode &start,
                                                    const TNode &goal) {
  ext::vector<TNode> path;
 
  // Path not found -> return empty vector
  if (p.find(goal) == p.end()) {
    return path;
  }
 
  path.push_back(goal);
  TNode current_vertex = goal;
 
  while (current_vertex != start) {
    current_vertex = p.at(current_vertex);
    path.push_back(current_vertex);
  }
 
  // Path need to be reverse.
  std::reverse(path.begin(), path.end());
 
  return path;
}
 
template<typename TNode>
ext::vector<TNode> ReconstructPath::reconstructPath(const ext::map<TNode, TNode> &p_forward,
                                                    const ext::map<TNode, TNode> &p_backward,
                                                    const TNode &start,
                                                    const TNode &goal,
                                                    const TNode &intersection_node) {
  ext::vector<TNode> path;
 
  // Path not found -> return empty vector
  if (p_forward.find(intersection_node) == p_forward.end()
      || p_backward.find(intersection_node) == p_backward.end()) {
    return path;
  }
 
  // First part of the path. From start to intersectionVertex (include).
  path.push_back(intersection_node);
  TNode current_vertex = intersection_node;
 
  while (current_vertex != start) {
    current_vertex = p_forward.at(current_vertex);
    path.push_back(current_vertex);
  }
  // This part of the path need to be reverse.
  std::reverse(path.begin(), path.end());
 
  // Second part of the path. From intersectionVertex (exclude) to goal.
  current_vertex = intersection_node;
  while (current_vertex != goal) {
    current_vertex = p_backward.at(current_vertex);
    path.push_back(current_vertex);
  }
  // No reverse at the end.
 
  return path;
}
 
// ---------------------------------------------------------------------------------------------------------------------
 
template<typename TNode, typename TWeight>
ext::pair<ext::vector<TNode>, TWeight>
ReconstructPath::reconstructWeightedPath(const ext::map<TNode, TNode> &p,
                                         const ext::map<TNode, TWeight> &g,
                                         const TNode &start,
                                         const TNode &goal) {
  if (g.find(goal) == g.end()) {
    return ext::make_pair(ext::vector<TNode>(), std::numeric_limits<TWeight>::max());
  }
 
  return ext::make_pair(reconstructPath(p, start, goal), g.at(goal));
}
 
template<typename TNode, typename TWeight>
ext::pair<ext::vector<TNode>, TWeight>
ReconstructPath::reconstructWeightedPath(const ext::map<TNode, TNode> &p_forward,
                                         const ext::map<TNode, TNode> &p_backward,
                                         const ext::map<TNode, TWeight> &g_forward,
                                         const ext::map<TNode, TWeight> &g_backward,
                                         const TNode &start,
                                         const TNode &goal,
                                         const TNode &intersection_node) {
  if (g_forward.find(intersection_node) == g_forward.end() || g_backward.find(intersection_node) == g_backward.end()) {
    return ext::make_pair(ext::vector<TNode>(), std::numeric_limits<TWeight>::max());
  }
 
  return ext::make_pair(reconstructPath(p_forward, p_backward, start, goal, intersection_node),
                        g_forward.at(intersection_node) + g_backward.at(intersection_node));
}
 
// ---------------------------------------------------------------------------------------------------------------------
 
template<typename TNode>
ext::vector<TNode> ReconstructPath::joinPath(ext::vector<TNode> &forward_path,
                                             const ext::vector<TNode> &backward_path) {
  // ++ because we want skip the insertion node -> already in forward_path
  forward_path.insert(forward_path.end(), ++backward_path.rbegin(), backward_path.rend());
  return forward_path;
}
 
// ---------------------------------------------------------------------------------------------------------------------
 
} // namespace graph::common