00001 #ifndef ASTARDIJKSTRA_H 00002 #define ASTARDIJKSTRA_H 00003 00004 #include <Structs/Trees/priorityQueue.h> 00005 #include <Utilities/geographic.h> 00006 00007 00020 template<class GraphType> 00021 class AStarDijkstra 00022 { 00023 public: 00024 typedef typename GraphType::NodeIterator NodeIterator; 00025 typedef typename GraphType::NodeDescriptor NodeDescriptor; 00026 typedef typename GraphType::EdgeIterator EdgeIterator; 00027 typedef typename GraphType::SizeType SizeType; 00028 typedef unsigned int WeightType; 00029 typedef PriorityQueue< WeightType, NodeIterator, HeapStorage> PriorityQueueType; 00030 00037 AStarDijkstra( GraphType& graph, unsigned int* timestamp):G(graph),m_timestamp(timestamp) 00038 { 00039 } 00040 00047 bool hasFeasiblePotentials( const typename GraphType::NodeIterator& t) 00048 { 00049 NodeIterator u,v,lastNode; 00050 EdgeIterator e,lastEdge; 00051 00052 WeightType potential_u, potential_v; 00053 int reducedCost; 00054 00055 for( u = G.beginNodes(), lastNode = G.endNodes(); u != lastNode; ++u) 00056 { 00057 for( e = G.beginEdges(u), lastEdge = G.endEdges(u); e != lastEdge; ++e) 00058 { 00059 v = G.target(e); 00060 potential_u = euclideanDistance( u->x, u->y, t->x, t->y); 00061 potential_v = euclideanDistance( v->x, v->y, t->x, t->y); 00062 reducedCost = e->weight - potential_u + potential_v; 00063 if( reducedCost < 0) 00064 { 00065 return false; 00066 } 00067 } 00068 } 00069 return true; 00070 } 00071 00079 WeightType runQuery( const typename GraphType::NodeIterator& s, const typename GraphType::NodeIterator& t) 00080 { 00081 NodeIterator u,v,lastNode; 00082 EdgeIterator e,lastEdge; 00083 00084 WeightType potential_u, potential_v; 00085 WeightType reducedCost; 00086 00087 assert(hasFeasiblePotentials(t)); 00088 00089 pq.clear(); 00090 ++(*m_timestamp); 00091 s->dist = 0; 00092 s->timestamp = (*m_timestamp); 00093 s->pred = G.nilNodeDescriptor(); 00094 00095 pq.insert( s->dist, s, &(s->pqitem)); 00096 00097 while( !pq.empty()) 00098 { 00099 u = pq.minItem(); 00100 pq.popMin(); 00101 if( u == t) break; 00102 00103 potential_u = euclideanDistance( u->x, u->y, t->x, t->y); 00104 for( e = G.beginEdges(u), lastEdge = G.endEdges(u); e != lastEdge; ++e) 00105 { 00106 v = G.target(e); 00107 potential_v = euclideanDistance( v->x, v->y, t->x, t->y); 00108 reducedCost = e->weight + potential_v - potential_u; 00109 00110 if( v->timestamp < (*m_timestamp)) 00111 { 00112 v->pred = u->getDescriptor(); 00113 v->timestamp = (*m_timestamp); 00114 00115 pq.insert( u->dist + reducedCost, v, &(v->pqitem)); 00116 v->dist = u->dist + reducedCost; 00117 } 00118 else if( v->dist > u->dist + reducedCost ) 00119 { 00120 v->pred = u->getDescriptor(); 00121 00122 pq.decrease( u->dist + reducedCost, &(v->pqitem)); 00123 v->dist = u->dist + reducedCost; 00124 } 00125 } 00126 } 00127 00128 u = t; 00129 t->dist = 0; 00130 while( u->pred != G.nilNodeDescriptor()) 00131 { 00132 v = G.getNodeIterator(u->pred); 00133 e = G.getEdgeIterator( v , u); 00134 t->dist += e->weight; 00135 u = v; 00136 } 00137 return t->dist; 00138 } 00139 00140 private: 00141 GraphType& G; 00142 unsigned int* m_timestamp; 00143 PriorityQueueType pq; 00144 }; 00145 00146 #endif//ASTARDIJKSTRA_H 00147