include/Algorithms/ShortestPath/aStarDijkstra.h

#ifndef ASTARDIJKSTRA_H
#define ASTARDIJKSTRA_H

#include <Structs/Trees/priorityQueue.h>
#include <Utilities/geographic.h>


template<class GraphType>
class AStarDijkstra
{
public:
    typedef typename GraphType::NodeIterator                        NodeIterator;
    typedef typename GraphType::NodeDescriptor                      NodeDescriptor;
    typedef typename GraphType::EdgeIterator                        EdgeIterator;
    typedef typename GraphType::SizeType                            SizeType;
    typedef unsigned int                                            WeightType;
    typedef PriorityQueue< WeightType, NodeIterator, HeapStorage>   PriorityQueueType;
    
    AStarDijkstra( GraphType& graph, unsigned int* timestamp):G(graph),m_timestamp(timestamp)
    {
    }
    
    bool hasFeasiblePotentials( const typename GraphType::NodeIterator& t)
    {
        NodeIterator u,v,lastNode;
        EdgeIterator e,lastEdge;

        WeightType potential_u, potential_v;
        int reducedCost;

        for( u = G.beginNodes(), lastNode = G.endNodes(); u != lastNode; ++u)
        {
            for( e = G.beginEdges(u), lastEdge = G.endEdges(u); e != lastEdge; ++e)
            {
                v = G.target(e);
                potential_u = euclideanDistance( u->x, u->y, t->x, t->y);
                potential_v = euclideanDistance( v->x, v->y, t->x, t->y);
                reducedCost = e->weight - potential_u + potential_v;
                if( reducedCost < 0)
                {
                    return false;
                }
            }       
        }
        return true;
    }
    
    WeightType runQuery( const typename GraphType::NodeIterator& s, const typename GraphType::NodeIterator& t)
    {
        NodeIterator u,v,lastNode;
        EdgeIterator e,lastEdge;
        
        WeightType potential_u, potential_v;
        WeightType reducedCost;
        
        assert(hasFeasiblePotentials(t));
        
        pq.clear();
        ++(*m_timestamp);
        s->dist = 0;
        s->timestamp = (*m_timestamp);
        s->pred = G.nilNodeDescriptor();

        pq.insert( s->dist, s, &(s->pqitem));

        while( !pq.empty())
        {
            u = pq.minItem();
            pq.popMin();
            if( u == t) break;
            
            potential_u = euclideanDistance( u->x, u->y, t->x, t->y);
            for( e = G.beginEdges(u), lastEdge = G.endEdges(u); e != lastEdge; ++e)
            {
                v = G.target(e);
                potential_v = euclideanDistance( v->x, v->y, t->x, t->y);
                reducedCost = e->weight + potential_v - potential_u; 
                
                if( v->timestamp < (*m_timestamp))
                {
                    v->pred = u->getDescriptor();
                    v->timestamp = (*m_timestamp);
                    
                    pq.insert( u->dist + reducedCost, v, &(v->pqitem));
                    v->dist = u->dist + reducedCost;
                }
                else if( v->dist > u->dist + reducedCost )
                {
                    v->pred = u->getDescriptor();
                    
                    pq.decrease( u->dist + reducedCost, &(v->pqitem));
                    v->dist = u->dist + reducedCost;
                }
            }
        }

        u = t;
        t->dist = 0;
        while( u->pred != G.nilNodeDescriptor())
        {
            v = G.getNodeIterator(u->pred);
            e = G.getEdgeIterator( v , u);
            t->dist += e->weight;
            u = v;
        }
        return t->dist;
    }
    
private:
    GraphType& G;
    unsigned int* m_timestamp;
    PriorityQueueType pq;
};

#endif//ASTARDIJKSTRA_H

---