0
\$\begingroup\$

I'm trying to implement the D*-Lite pathfinding algorithm, as described in the 2002 article by Koenig and Likhachev for grid-based navgraph. But I stuck with problem. My ComputeShortestPath (in my implementation it is Search() function) can't find the path to source and visited nodes looks very similar non-dependent from source and target positions (it always seeks diagonally to left and top from target node).

Looks like that I have some bug here but I can't find it by myself. If somebody can tell me what is wrong in this code, I will be very happy.

Thank you very much.

class DStarLiteKey
{
public:
    DStarLiteKey(double left = 0, double right = 0) : left(left), right(right) {}

    bool operator==(const DStarLiteKey &other) const
    {
        return isEqual(left, other.left) && isEqual(right, other.right);
    }

    bool operator > (const DStarLiteKey &other) const {
        if (left - EPSILON > other.left) return true;
        else if (left < other.left - EPSILON) return false;
        return right > other.right;
    }

    bool operator <= (const DStarLiteKey &other) const {
        if (left < other.left) return true;
        else if (left > other.left) return false;
        return right < other.right + EPSILON;
    }

    bool operator < (const DStarLiteKey &other) const {
        if (left + EPSILON < other.left) return true;
        else if (left - EPSILON > other.left) return false;
        return right < other.right;
    }

public:
    double left, right;

protected:
    static constexpr double EPSILON = std::numeric_limits<double>::epsilon();
};

class HeuristicEightCondist
{
public:
    HeuristicEightCondist(){}

    static double calculate(const Graph &graph, IdxType node1, IdxType node2)
    {
        Graph::NodeType nd1 = graph.GetNode(node1);
        Graph::NodeType nd2 = graph.GetNode(node2);
        double temp;
        double min = fabs(nd1.position.x - nd2.position.x);
        double max = fabs(nd1.position.y - nd2.position.y);
        if (min > max)
        {
            temp = min;
            min = max;
            max = temp;
        }
        return ((M_SQRT2-1.0)*min + max);
    }
};

template <class Heuristic>
class DStarLiteSearch
{
public:
    static constexpr double M_INF = std::numeric_limits<double>::min();
    static constexpr double INF = std::numeric_limits<double>::max();

public:
    DStarLiteSearch(Graph &graph) :
        m_graph(graph),
        m_pq(),
        m_nodePqIndeces(graph.numNodes()),
        m_pqNodes(graph.numNodes()),
        m_source(-1),
        m_target(-1),
        m_pathExists(false)
    {}

    void Init(IdxType source, IdxType target)
    {
        m_pq.init(m_graph.numNodes(), DStarLiteKey(M_INF, M_INF),
                                      DStarLiteKey(INF, INF));
        m_km = 0;
        m_gCosts = std::vector<double>(m_graph.numNodes(), std::numeric_limits<double>::max());
        m_rhsCosts = std::vector<double>(m_graph.numNodes(), std::numeric_limits<double>::max());
        m_nodePqIndeces = std::vector<IdxType>(m_graph.numNodes(), 0);
        m_pqNodes = std::vector<IdxType>(m_graph.numNodes(), 0);
        m_source = source;
        m_target = target;
        m_pathExists = false;

        // Algorithm begins
        m_rhsCosts[m_target] = 0;

        IdxType nodeIdx = m_pq.insert(DStarLiteKey(Heuristic::calculate(m_graph, m_source, m_target), 0));
        m_nodePqIndeces[nodeIdx] = m_target;
        m_pqNodes[m_target] = nodeIdx;
    }

    bool Search();

    std::list<IdxType> GetPath();

#ifdef DEBUG
    //returns a vector containing pointers to all the edges the search has examined
    std::vector<const Edge*> GetSearchTree()const{return m_spanningTree;}
#endif

    bool isPathExist()
    {
        return m_pathExists;
    }

protected:
    DStarLiteKey calculateKey(IdxType node)
    {
        double temp = std::min(m_gCosts[node], m_rhsCosts[node]);
        return DStarLiteKey(temp + Heuristic::calculate(m_graph, m_source, node) + m_km, temp);
    }

    void updateVertex(IdxType node)
    {
        bool isInPQ = m_pq.isObjectExist(m_pqNodes[node]);

        if(!isEqual(m_gCosts[node], m_rhsCosts[node]) && isInPQ)
        {
            m_pq.update(m_pqNodes[node], calculateKey(node));
        }
        else if(!isEqual(m_gCosts[node], m_rhsCosts[node]) && !isInPQ)
        {
            IdxType nodeIdx = m_pq.insert(calculateKey(node));
            m_nodePqIndeces[nodeIdx] = node;
            m_pqNodes[node] = nodeIdx;
        }
        else if(isEqual(m_gCosts[node], m_rhsCosts[node]) && isInPQ)
        {
            m_pq.remove(m_pqNodes[node]);
        }
    }

protected:
    Graph &m_graph;
    MinHeap<DStarLiteKey, IdxType> m_pq;
    std::vector<IdxType> m_nodePqIndeces;
    std::vector<IdxType> m_pqNodes;
    std::vector<double> m_gCosts;
    std::vector<double> m_rhsCosts;
    double m_km;
    NodeIndex m_source;
    NodeIndex m_target;
    bool m_pathExists;

#ifdef DEBUG
    //As the search progresses, this will hold all the edges the algorithm has
    //examined. THIS IS NOT NECESSARY FOR THE SEARCH, IT IS HERE PURELY
    //TO PROVIDE THE USER WITH SOME VISUAL FEEDBACK
    std::vector<const Edge*>  m_spanningTree;
#endif
};

#endif // GRAPHALGORITHMS_H

template<class Heuristic>
bool DStarLiteSearch<Heuristic>::Search()
{
#ifdef DEBUG
    m_spanningTree.clear();
#endif

    IdxType node;
    DStarLiteKey kOld, kNew;
    double gOld, minSuccessorCost, temp;

    while(m_pq.min() < calculateKey(m_source) || m_rhsCosts[m_source] > m_gCosts[m_source])
    {
        node = m_nodePqIndeces[m_pq.minIdx()];
        kOld = m_pq.min();
        kNew = calculateKey(node);

        if(kOld < kNew)
        {
            m_pq.update(m_pqNodes[node], kNew);
        }
        else if(m_gCosts[node] > m_rhsCosts[node])
        {
            m_gCosts[node] = m_rhsCosts[node];
            m_pq.remove(m_pqNodes[node]);

            Graph::ConstPredecessorEdgeIterator edges(m_graph, node);
            for (const Graph::EdgeType * curEdge = edges.begin();
                        !edges.end();
                        curEdge = edges.next())
            {
                m_spanningTree.push_back(curEdge);

                if(curEdge->from != m_target)
                {
                    m_rhsCosts[curEdge->from] = std::min(m_rhsCosts[curEdge->from], curEdge->cost + m_gCosts[node]);
                    updateVertex(curEdge->from);
                }
            }
        }
        else
        {
            gOld = m_gCosts[node];
            m_gCosts[node] = INF;

            Graph::ConstPredecessorEdgeIterator edges(m_graph, node);
            for (const Graph::EdgeType * curEdge = edges.begin();
                        !edges.end();
                        curEdge = edges.next())
            {
                m_spanningTree.push_back(curEdge);

                if(m_rhsCosts[curEdge->from] == curEdge->cost + gOld)
                {
                    if(curEdge->from != m_target)
                    {
                        minSuccessorCost = INF;

                        Graph::ConstEdgeIterator edges(m_graph, curEdge->from);
                        for (const Graph::EdgeType * edge = edges.begin();
                                    !edges.end();
                                    curEdge = edges.next())
                        {
                            //m_spanningTree.push_back(edge);

                            temp = edge->cost + m_gCosts[edge->to];

                            if(minSuccessorCost > temp)
                            {
                                minSuccessorCost = temp;
                            }
                        }
                        m_rhsCosts[curEdge->from] = minSuccessorCost;
                    }
                }
                updateVertex(curEdge->from);
            }

            //updateVertex(node);
        }
    }

    if(!isEqual(m_rhsCosts[m_source], INF))
    {
        m_pathExists = true;
        return true;
    }

    return false;
}
\$\endgroup\$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.