Wegfindung mit A*

#include "shortest_path.hpp"

#include <set>
#include <unordered_set>
#include <array>
#include <cassert>

typedef sf::Vector2i vec;   // sf::Vector2i hat die ints x und y als Member
typedef unsigned f_t;
typedef std::vector<std::vector<bool> > map_t;
typedef std::list<vec> short_path_result_t;

struct Node
{
    Node(vec pos_, const Node* parent_, f_t g_, f_t h_): pos(pos_), parent(parent_), g(g_), h(h_) { }
    Node(vec pos_): pos(pos_) { }   // Alles außer pos bleibt uninitalisiert!
    bool operator< (const Node& rhs) const  { return f() < rhs.f(); }
    bool operator== (const Node& rhs) const { return pos == rhs.pos;  }
    f_t f() const                           { return g + h; }

    vec pos;
    const Node* parent;
    f_t g, h;
};

namespace std {
template<>
struct hash<vec>
{
    std::size_t operator() (const vec& v) const
    {
        static const std::hash<int> hash_impl;
        return hash_impl((v.x << 16) + v.y);
    }
};
}

namespace std {
template<>
struct hash<Node>
{
    std::size_t operator() (const Node& v) const
    {
        static const std::hash<vec> hash_impl;
        return hash_impl(v.pos);
    }
};
}

namespace std {
template<>
struct hash<std::pair<const vec*, std::multiset<Node>::const_iterator> >
{
    std::size_t operator() (const std::pair<const vec*, std::multiset<Node>::const_iterator> nd) const
    {
        static const std::hash<vec> hash_impl;
        return hash_impl(*nd.first);
    }
};
}


inline unsigned calc_h(vec from, vec to)
{
    const vec d = to - from;
    return (abs(d.x) + abs(d.y)) * 10;
}

const short_path_result_t walk_back(const Node* last)
{
    short_path_result_t result;
    while (last != nullptr)
    {
        result.push_front(last->pos);
        last = last->parent;
    }
    return result;
}

const short_path_result_t shortest_path(const vec from, const vec to, const map_t& map, const bool walkable_val)
{
    if (map[from.x][from.y] != walkable_val || map[to.x][to.y] != walkable_val)
        return short_path_result_t();

    typedef std::unordered_set<Node> closed_t;  // Sortiert nach Position
    typedef std::multiset<Node> open_t;         // Sortiert nach f-Wert

    closed_t closed;
    open_t open;

    open.insert(Node(from, nullptr, 0, calc_h(from, to)));

    while (!open.empty())
    {
        // Wähle Knoten mit bestem f aus offenen aus und verschiebe ihn zu den geschlossenen.
        const std::pair<closed_t::const_iterator, bool> ins_res =
            closed.insert(*open.begin());
        assert(ins_res.second);
        open.erase(open.begin());
        const Node& cur = *ins_res.first;

        if (cur.pos == to) return walk_back(&cur);

        typedef std::tr1::array<vec, 8> dirs_t;
        static const dirs_t dirs = {vec( 0, +1), vec(+1,  0), vec( 0, -1), vec(-1,  0),     // orthogonal; g = 10
                                    vec(+1, +1), vec(+1, -1), vec(-1, +1), vec(-1, -1)};    // diagonal;   g = 14
        for (dirs_t::size_type i = 0; i < dirs.size(); ++i)
        {
            const vec next_pos = cur.pos + dirs[i];

            // Ist next_pos begehbar & vorhanden?
            if (next_pos.x >= 0 && next_pos.y >= 0 &&
                next_pos.x < map.size() && next_pos.y < map.front().size() &&
                map[next_pos.x][next_pos.y] == walkable_val && (
                    i < 4 || (
                        map[next_pos.x][cur.pos.y] == walkable_val &&
                        map[cur.pos.x][next_pos.y] == walkable_val
                    )
                ) && closed.find(Node(next_pos)) == closed.end() // Ist next_pos bereits geschlosssen?
            )
            {
                const Node next(next_pos, &cur, cur.g + (i < 4 ? 10 : 14), calc_h(next_pos, to));

                const open_t::const_iterator it = std::find_if(
                    open.begin(), open.end(),
                    [next_pos](const open_t::value_type& val) { return val.pos == next_pos; }
                );

                // Ist next_pos bereits geöffnet?
                if (it != open.end())
                {
                    // Ist dieser Weg zu next_pos besser, als der bisherige?
                    if (next.g < it->g)
                    {
                        // Wenn, ja parent zu &cur ändern, und g neu berechnen (oben geschehen).
                        open.erase(it);
                        open.insert(next);
                    }
                }
                // Wenn next_pos neu entdeckt wurde, d.h nicht geöffnet, geschlossen oder unbegehbar ist:
                else
                {
                    open.insert(next);
                }
            }   // if (vorhanden & begehbar)
        }   // for (dirs)
    }   // while (!open.empty)
    return short_path_result_t();   // Keine geöffnenten Knoten mehr und to nicht gefunden: Rückgabe ist leerer Pfad.
}