D* Lite implementation in C++ (experimental)

dstarlite.cpp

////
// D* Lite implementation in C++
// @tokoro10g
// Reference: http://idm-lab.org/bib/abstracts/papers/aaai02b.pdf
////

#include <bits/stdc++.h>
using namespace std;

using Cost = float;
constexpr Cost inf = numeric_limits<Cost>::infinity();
constexpr Cost large = inf;

//using Cost = unsigned int;
//constexpr Cost large = 4e6;
//constexpr Cost inf = 4e6;

Cost key_modifier;

using Edge = pair<unsigned int, unsigned int>;
using HeapKey = pair<Cost, Cost>;

vector<pair<HeapKey, unsigned int>> U;
int in_U[10000];
map<Edge, bool> observed_edge;

unsigned int id_start_orig = 0;
unsigned int id_start;
unsigned int id_goal = 1;
unsigned int id_last;

struct KeyCompare {
    bool operator()(const pair<HeapKey, unsigned int>& a, const pair<HeapKey, unsigned int>& b) const
    {
        return a.first < b.first;
    }
};

Cost heuristic(int id1, int id2)
{
    const int w = 3;
    int x1 = id1 % w, y1 = id1 / w;
    int x2 = id2 % w, y2 = id2 / w;
    return max(abs(x1 - x2), abs(y1 - y2));
    //return sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
}

class Node {
public:
    Node(const int id)
        : id(id)
        , neighbors()
        , g(inf)
        , rhs(inf) {};
    const unsigned int id;
    vector<Node*> neighbors;
    Cost g;
    Cost rhs;

    const HeapKey calculateKey() const
    {
        return { min(g, rhs) + heuristic(id_start, id) + key_modifier, min(g, rhs) };
    }
};

class Graph {
public:
    Graph(const unsigned int size)
        : size(size)
    {
        assert(size > 2);
        for (unsigned int i = 0; i < size; i++) {
            nodes.push_back(new Node(i));
        }
    }
    ~Graph()
    {
        for (unsigned int i = 0; i < size; i++) {
            delete nodes[i];
        }
    }
    void connect(const unsigned int id1, const unsigned int id2, const int cost)
    {
        assert(id1 < size && id2 < size);
        // undirected graph
        nodes[id1]->neighbors.push_back(nodes[id2]);
        nodes[id2]->neighbors.push_back(nodes[id1]);
        edges.insert({ makeEdgeKey(id1, id2), cost });
    }
    const bool edgeExists(const unsigned int id1, const unsigned int id2) const
    {
        assert(id1 < size && id2 < size);
        return edges.find(makeEdgeKey(id1, id2)) != edges.end();
    }
    const Cost getEdgeCost(const unsigned int id1, const unsigned int id2) const
    {
        assert(id1 < size && id2 < size);
        assert(edgeExists(id1, id2));
        return edges.at(makeEdgeKey(id1, id2));
    }
    void setEdgeCost(const unsigned int id1, const unsigned int id2, const Cost cost)
    {
        assert(id1 < size && id2 < size);
        assert(edgeExists(id1, id2));
        edges.at(makeEdgeKey(id1, id2)) = cost;
    }
    Node* getNode(const unsigned int id)
    {
        assert(id < size);
        return nodes[id];
    }
    void setNodeG(const unsigned int id, const Cost g)
    {
        assert(id < size);
        nodes[id]->g = g;
    }
    void setNodeRhs(const unsigned int id, const Cost rhs)
    {
        assert(id < size);
        nodes[id]->rhs = rhs;
    }

    static const Edge makeEdgeKey(const unsigned int id1, const unsigned int id2)
    {
        return { min(id1, id2), max(id1, id2) };
    }

public:
    const unsigned int size;

private:
    vector<Node*> nodes;
    map<Edge, Cost> edges;
};

Graph graph(9);
Graph vgraph(9);

ostream& operator<<(ostream& os, pair<Cost, Cost> const& p)
{
    return os << "<" << p.first << ", " << p.second << ">";
}

ostream& operator<<(ostream& os, Node const& n)
{
    return os << n.id << make_pair(n.g, n.rhs);
}

ostream& operator<<(ostream& os, Graph& g)
{
    for (unsigned int i = 0; i < g.size; i++) {
        Node* n = g.getNode(i);
        os << *n << ": ";
        for (auto nn : n->neighbors) {
            os << nn->id << "(" << g.getEdgeCost(nn->id, i) << "|" << nn->g + g.getEdgeCost(nn->id, i) << ") ";
        }
        os << endl;
    }
    return os;
}

void updateHeap(unsigned int id, HeapKey k)
{
    cout << "***updateHeap(" << id << ", " << k << ")" << endl;
    for (auto& p : U) {
        if (p.second == id) {
            p.first = k;
            make_heap(U.begin(), U.end(), KeyCompare());
            return;
        }
    }
}

void insertHeap(unsigned int id, HeapKey k)
{
    U.push_back({ k, id });
    push_heap(U.begin(), U.end(), KeyCompare());
    in_U[id]++;
}

void updateVertex(unsigned int id)
{
    Node* n = vgraph.getNode(id);
    if (n->g != n->rhs && in_U[id]) {
        updateHeap(id, n->calculateKey());
    } else if (n->g != n->rhs && !in_U[id]) {
        insertHeap(id, n->calculateKey());
    } else if (n->g == n->rhs && in_U[id]) {
        auto it = find_if(U.begin(), U.end(), [=](auto p) { return p.second == id; });
        U.erase(it);
        in_U[id]--;
    }
}

void computeShortestPath()
{
    cout << "***computeShortestPath()" << endl;
    Node* n = vgraph.getNode(id_start);
    while (U.front().first < n->calculateKey() || n->rhs > n->g) {
        cout << "Contents of the heap:" << endl;
        for (auto p : U) {
            cout << p.second << p.first << endl;
        }
        cout << endl;
        auto uid = U.front().second;
        Node* u = vgraph.getNode(uid);
        auto kold = U.front().first;
        auto knew = u->calculateKey();
        if (kold < knew) {
            updateHeap(u->id, knew);
        } else if (u->g > u->rhs) {
            u->g = u->rhs;
            pop_heap(U.begin(), U.end(), KeyCompare());
            U.pop_back();
            in_U[u->id]--;
            for (auto s : u->neighbors) {
                if (s->id != id_goal) {
                    s->rhs = min(s->rhs, vgraph.getEdgeCost(u->id, s->id) + u->g);
                }
                updateVertex(s->id);
            }
        } else {
            Cost gold = u->g;
            u->g = inf;
            for (auto s : u->neighbors) {
                if (s->rhs == vgraph.getEdgeCost(s->id, u->id) + gold) {
                    if (s->id != id_goal) {
                        Cost mincost = inf;
                        for (auto sp : s->neighbors) {
                            mincost = min(mincost, vgraph.getEdgeCost(sp->id, s->id) + sp->g);
                        }
                        s->rhs = mincost;
                    }
                }
                updateVertex(s->id);
            }
            if (u->rhs == gold) {
                if (u->id != id_goal) {
                    Cost mincost = inf;
                    for (auto sp : u->neighbors) {
                        mincost = min(mincost, vgraph.getEdgeCost(sp->id, u->id) + sp->g);
                    }
                    u->rhs = mincost;
                }
            }
            updateVertex(u->id);
        }
    }
}

int main(int argc, char* argv[])
{
    graph.connect(0, 3, 1);
    graph.connect(3, 6, 1);
    graph.connect(6, 7, 1);
    graph.connect(7, 8, 1);
    graph.connect(8, 5, 1);
    graph.connect(5, 4, 1);
    graph.connect(5, 2, 1);
    graph.connect(1, 4, 1);

    cout << graph << endl;

    vgraph.connect(0, 3, 1);
    vgraph.connect(3, 4, 1);
    vgraph.connect(3, 6, 1);
    vgraph.connect(6, 7, 1);
    vgraph.connect(7, 8, 1);
    vgraph.connect(7, 4, 1);
    vgraph.connect(8, 5, 1);
    vgraph.connect(5, 4, 1);
    vgraph.connect(5, 2, 1);
    vgraph.connect(1, 2, 1);
    vgraph.connect(1, 4, 1);

    cout << vgraph << endl;

    //////////////////////////
    id_start = id_start_orig;
    id_last = id_start;
    key_modifier = 0;
    vgraph.setNodeRhs(id_goal, 0);

    insertHeap(id_goal, { heuristic(id_start, id_goal), 0 });

    computeShortestPath();

    while (id_start != id_goal) {
        cout << vgraph << endl;
        Node* start = vgraph.getNode(id_start);
        if (start->rhs == inf) {
            cerr << "NO ROUTE!!!" << endl;
            return -1;
        }

        unsigned int argmin = id_start;
        Cost mincost = inf;
        for (auto sp : start->neighbors) {
            Cost cost = vgraph.getEdgeCost(id_start, sp->id) + sp->g;
            cout << "Cost from " << id_start << " to " << sp->id << ": " << cost << endl;
            if (mincost > cost) {
                mincost = cost;
                argmin = sp->id;
            }
        }
        cout << "Move from " << id_start << " to " << argmin << endl;
        id_start = argmin;
        Node* start_new = vgraph.getNode(id_start);

        bool cost_changed = false;
        vector<Edge> changed_edges;
        for (auto sp : start_new->neighbors) {
            Edge edge = Graph::makeEdgeKey(id_start, sp->id);
            if (observed_edge.find(edge) == observed_edge.end()) {
                observed_edge.insert({ edge, graph.edgeExists(id_start, sp->id) });
                changed_edges.push_back(Graph::makeEdgeKey(id_start, sp->id));
                cost_changed = true;
                cout << id_start << " to " << sp->id << ": " << (graph.edgeExists(id_start, sp->id) ? "isle" : "wall") << endl;
            }
        }
        if (cost_changed) {
            key_modifier += heuristic(id_last, id_start);
            id_last = id_start;
            for (auto e : changed_edges) {
                Cost cold = vgraph.getEdgeCost(e.first, e.second);
                vgraph.setEdgeCost(e.first, e.second, observed_edge.at(e) ? 1 : large);
                auto u = vgraph.getNode(e.first);
                auto v = vgraph.getNode(e.second);
                if (cold > large) {
                    if (e.first != id_goal) {
                        u->rhs = min(u->rhs, large + v->g);
                    }
                } else if (u->rhs == cold + v->g) {
                    if (e.first != id_goal) {
                        Cost mincost = inf;
                        for (auto sp : u->neighbors) {
                            mincost = min(mincost, vgraph.getEdgeCost(sp->id, e.first) + sp->g);
                        }
                        u->rhs = mincost;
                    }
                }
                updateVertex(e.first);
                if (cold > large) {
                    if (e.second != id_goal) {
                        v->rhs = min(v->rhs, large + u->g);
                    }
                } else if (v->rhs == cold + u->g) {
                    if (e.second != id_goal) {
                        Cost mincost = inf;
                        for (auto sp : v->neighbors) {
                            mincost = min(mincost, vgraph.getEdgeCost(sp->id, e.second) + sp->g);
                        }
                        v->rhs = mincost;
                    }
                }
                updateVertex(e.second);
            }
            computeShortestPath();
        }
    }
    computeShortestPath();
    cout << vgraph << endl;

    return 0;
}