OdinAI
 All Classes Namespaces Functions Variables
AStarGraphSearch.h
1 /*******************************************************************************
2  * ________ .___.__ _____ .___
3  * \_____ \ __| _/|__| ____ / _ \ | |
4  * / | \ / __ | | |/ \ / /_\ \| |
5  * / | \/ /_/ | | | | \/ | \ |
6  * \_______ /\____ | |__|___| /\____|__ /___|
7  * \/ \/ \/ \/
8  *
9  * Copyright (c) Emil Sandstø 2012
10  *******************************************************************************/
11 #ifndef ODINAI_ASTAR_GRAPH_SEARCH_H_
12 #define ODINAI_ASTAR_GRAPH_SEARCH_H_
13 
14 #include <vector>
15 
16 #include "PriorityQueue.h"
17 #include <list>
18 
19 namespace OdinAI
20 {
26 template<class GRAPH_TYPE, class HEURISTIC_TYPE>
28 {
29 public:
30 
31 private:
32  typedef typename GRAPH_TYPE::EdgeType Edge;
33  typedef typename GRAPH_TYPE::NodeType Node;
34 
35  const GRAPH_TYPE &m_graph;
36 
37  std::vector<int> m_realCosts;
38  std::vector<int> m_finalCosts;
39 
40  std::vector<const Edge*> m_shortestPathTree;
41  std::vector<const Edge*> m_searchFrontier;
42 
43  int m_sourceNode;
44  int m_targetNode;
45 
49  void Search();
50 public:
51  AStarGraphSearch(const GRAPH_TYPE &graph, int sourceNode, int targetNode) : m_graph(graph),
52  m_shortestPathTree(graph.NumNodes()),
53  m_searchFrontier(graph.NumNodes()),
54  m_realCosts(graph.NumNodes(), 0),
55  m_finalCosts(graph.NumNodes(), 0),
56  m_sourceNode(sourceNode),
57  m_targetNode(targetNode)
58  {
59  //Do the search.
60  Search();
61  }
62 
66  std::vector<const Edge*> GetSPT() const
67  {
68  return m_shortestPathTree;
69  }
70 
74  void GetPath(std::list<int> &outPath) const
75  {
76  int node = m_targetNode;
77  //If we did not find the target node, return.
78  if(m_shortestPathTree[node] == 0)
79  return;
80 
81  outPath.push_front(node);
82 
83  while(node != m_sourceNode && m_shortestPathTree[node] != 0)
84  {
85  node = m_shortestPathTree[node]->From();
86  outPath.push_front(node);
87  }
88  }
89 
93  double GetCostToTarget() const
94  {
95  return m_realCosts[m_targetNode];
96  }
97 };
98 
99 template<class GRAPH_TYPE, class HEURISTIC_TYPE>
100 void AStarGraphSearch<GRAPH_TYPE, HEURISTIC_TYPE>::Search()
101 {
102  IndexedPriorityQLow<int> pQueue(m_finalCosts, m_graph.NumNodes());
103 
104  pQueue.Insert(m_sourceNode);
105 
106  while(!pQueue.Empty())
107  {
108  int nextClosestNode = pQueue.Pop();
109 
110  m_shortestPathTree[nextClosestNode] = m_searchFrontier[nextClosestNode];
111 
112  if(nextClosestNode == m_targetNode)
113  {
114  return;
115  }
116 
117  GRAPH_TYPE::ConstEdgeIterator constEdgeIter(m_graph, nextClosestNode);
118  for(const Edge *edge = constEdgeIter.Begin();
119  !constEdgeIter.End();edge = constEdgeIter.Next())
120  {
121  int realCost = m_realCosts[nextClosestNode] + edge->Cost();
122  int heuristicCost = HEURISTIC_TYPE::Calculate<GRAPH_TYPE>(m_graph, m_targetNode, edge->To());
123 
124  //If we have not added the edge yet, add it.
125  if(m_searchFrontier[edge->To()] == nullptr)
126  {
127  m_finalCosts[edge->To()] = realCost + heuristicCost;
128  m_realCosts[edge->To()] = realCost;
129 
130  pQueue.Insert(edge->To());
131 
132  m_searchFrontier[edge->To()] = edge;
133  }
134  //If we found a cheaper way to this node, change the costs accordingly.
135  else if(realCost < m_realCosts[edge->To()] &&
136  (m_shortestPathTree[edge->To()] == nullptr))
137  {
138  m_finalCosts[edge->To()] = realCost + heuristicCost;
139  m_realCosts[edge->To()] = realCost;
140 
141  pQueue.ChangePriority(edge->To());
142 
143  m_searchFrontier[edge->To()] = edge;
144  }
145  }
146  }
147 }
148 }
149 
150 #endif
std::vector< const Edge * > GetSPT() const
Definition: AStarGraphSearch.h:66
double GetCostToTarget() const
Definition: AStarGraphSearch.h:93
Definition: AStarGraphSearch.h:27
void GetPath(std::list< int > &outPath) const
Definition: AStarGraphSearch.h:74