11 #ifndef ODINAI_ASTAR_GRAPH_SEARCH_H_
12 #define ODINAI_ASTAR_GRAPH_SEARCH_H_
16 #include "PriorityQueue.h"
26 template<
class GRAPH_TYPE,
class HEURISTIC_TYPE>
32 typedef typename GRAPH_TYPE::EdgeType Edge;
33 typedef typename GRAPH_TYPE::NodeType Node;
35 const GRAPH_TYPE &m_graph;
37 std::vector<int> m_realCosts;
38 std::vector<int> m_finalCosts;
40 std::vector<const Edge*> m_shortestPathTree;
41 std::vector<const Edge*> m_searchFrontier;
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)
66 std::vector<const Edge*>
GetSPT()
const
68 return m_shortestPathTree;
74 void GetPath(std::list<int> &outPath)
const
76 int node = m_targetNode;
78 if(m_shortestPathTree[node] == 0)
81 outPath.push_front(node);
83 while(node != m_sourceNode && m_shortestPathTree[node] != 0)
85 node = m_shortestPathTree[node]->From();
86 outPath.push_front(node);
95 return m_realCosts[m_targetNode];
99 template<
class GRAPH_TYPE,
class HEURISTIC_TYPE>
100 void AStarGraphSearch<GRAPH_TYPE, HEURISTIC_TYPE>::Search()
102 IndexedPriorityQLow<int> pQueue(m_finalCosts, m_graph.NumNodes());
104 pQueue.Insert(m_sourceNode);
106 while(!pQueue.Empty())
108 int nextClosestNode = pQueue.Pop();
110 m_shortestPathTree[nextClosestNode] = m_searchFrontier[nextClosestNode];
112 if(nextClosestNode == m_targetNode)
117 GRAPH_TYPE::ConstEdgeIterator constEdgeIter(m_graph, nextClosestNode);
118 for(
const Edge *edge = constEdgeIter.Begin();
119 !constEdgeIter.End();edge = constEdgeIter.Next())
121 int realCost = m_realCosts[nextClosestNode] + edge->Cost();
122 int heuristicCost = HEURISTIC_TYPE::Calculate<GRAPH_TYPE>(m_graph, m_targetNode, edge->To());
125 if(m_searchFrontier[edge->To()] ==
nullptr)
127 m_finalCosts[edge->To()] = realCost + heuristicCost;
128 m_realCosts[edge->To()] = realCost;
130 pQueue.Insert(edge->To());
132 m_searchFrontier[edge->To()] = edge;
135 else if(realCost < m_realCosts[edge->To()] &&
136 (m_shortestPathTree[edge->To()] ==
nullptr))
138 m_finalCosts[edge->To()] = realCost + heuristicCost;
139 m_realCosts[edge->To()] = realCost;
141 pQueue.ChangePriority(edge->To());
143 m_searchFrontier[edge->To()] = edge;
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