Keskustelu: Koodit: A*-hakualgoritmi C++:lla Python-moduuliksi

#include <iostream>
#include <string>
#include <vector>
#include <algorithm>

#include <boost/foreach.hpp>
#include <boost/python.hpp>

#define foreach BOOST_FOREACH
#define reverse_foreach BOOST_REVERSE_FOREACH

// spice things up
using namespace std;
using namespace boost;

/**
 * PathNode is used for in the A* algorithm to represent
 * different locations.
 */
struct PathNode
{
    PathNode()
        : f(0), g(0), h(0), x(0), y(0), parent(NULL)
    {}

    PathNode(int x, int y)
        : f(0), g(0), h(0), x(x), y(y), parent(NULL)
    {}

    bool operator<(const PathNode& node)
    {
        return f < node.f;
    }

    bool operator>(const PathNode& node)
    {
        return f > node.f;
    }

    bool operator==(const PathNode& node)
    {
        return (x == node.x && y == node.y);
    }

    bool operator!=(const PathNode& node)
    {
        return !operator==(node);
    }

    void update() { f = g + h; }

    // f = g + h
    int f;

    // the cost of movement from starting node to this node along the
    // generated path
    int g;

    // the cost of movement from this node to the goal node ("heuristic")
    int h;

    // this node's location
    int x, y;

    // used for traversing the generated path
    PathNode *parent;
};

struct PathNodePtrCmp: public binary_function<PathNode*, PathNode*, bool>
{
    bool operator()(PathNode* a, PathNode* b)
    {
        return (*a > *b);
    }
};

// a little helper struct to manage our nodes
struct PathNodeHeap
{
    void push(PathNode* v)
    {
        heap.push_back(v);
        push_heap(heap.begin(), heap.end(), PathNodePtrCmp());
    }

    PathNode* pop()
    {
        pop_heap(heap.begin(), heap.end(), PathNodePtrCmp());
        PathNode* res = heap.back();
        heap.pop_back();
        return res;
    }

    PathNode* top()
    {
        return heap.front();
    }

    PathNode* find(int x, int y)
    {
        PathNode* node;

        foreach(node, heap)
        {
            if (node->x == x && node->y == y)
            {
                return node;
            }
        }

        return NULL;
    }

    void sort()
    {
        sort_heap(heap.begin(), heap.end(), PathNodePtrCmp());
    }

    bool empty()
    {
        return heap.empty();
    }

    vector<PathNode*> heap;
};

// here is the algorithm
python::object search_path(python::object map, python::tuple start,
                           python::tuple goal)
{
    PathNodeHeap open, closed;

    PathNode* current = NULL;

    PathNode* first = new PathNode(python::extract<int>(start[0]),
                                   python::extract<int>(start[1]));

    PathNode dest = PathNode(python::extract<int>(goal[0]),
                             python::extract<int>(goal[1]));

    open.push(first);

    // stop, when the open list is empty (path was not found)
    while (!open.empty())
    {
        // take the node that has the lowest f-score from the open list
        // and switch it to the closed list
        current = open.pop();
        closed.push(current);

        // when the goal node is switched to the closed list,
        // we have found our path
        if (*current == dest)
        {
            break;
        }

        // we'll want to check every 8 directions
        int directions[][2] = { {-1, 0}, {1, 0},
                                {0, -1}, {0, 1},
                                {-1, -1}, {1, 1},
                                {1, -1}, {-1, 1} };

        for (int i = 0; i < 8; ++i)
        {
            int dx = directions[i][0];
            int dy = directions[i][1];

            int x = current->x + dx;
            int y = current->y + dy;

            bool walkable = python::extract<bool>(
                map.attr("walkable")(x, y));

            // ignore if on closed list and if not walkable
            if (!closed.find(x, y) && walkable)
            {
                PathNode* node = open.find(x, y);

                if (node == NULL)
                {
                    // node wasn't on the open list,
                    // so add it to there

                    node = new PathNode(x, y);

                    node->parent = current;

                    // the cost of movement is bigger when movement is diagonal
                    if (i > 3)
                    {
                        node->g = current->g + 14;
                    }
                    else
                    {
                        node->g = current->g + 10;
                    }

                    node->h = abs(dest.x - x) + abs(dest.y - y);
                    node->update();

                    open.push(node);
                }
                else
                {
                    // node was found from the open list,
                    // check if current path is better ...

                    int newg = current->g;

                    if (i > 3)
                    {
                        newg += 14;
                    }
                    else
                    {
                        newg += 10;
                    }

                    if (newg < node->g)
                    {
                        node->g = newg;
                        node->parent = current;
                        node->update();
                        open.sort();
                    }
                }
            }
        }

        // if current is NULL after this loop, there was no path found
        current = NULL;
    }

    // collect the results in a nice python list

    python::list result;

    while (current)
    {
        result.append(python::make_tuple(current->x, current->y));
        current = current->parent;
    }

    foreach (PathNode *p, open.heap)
    {
        delete p;
    }

    foreach (PathNode *p, closed.heap)
    {
        delete p;
    }

    return result;
}

BOOST_PYTHON_MODULE(astarcpp)
{
    python::def("search", search_path);
}

#!/usr/bin/env python

from __future__ import print_function
import astarcpp

class GameMap:
    def __init__(self, filename):
        self.data = []
        self.load(filename)

    def load(self, filename):
        f = open(filename)
        lines = f.readlines()
        f.close()

        for line in lines:
            line = line.strip()
            tmp = map(ord, line)
            self.data.append(tmp)

        self.w = len(self.data[0])
        self.h = len(self.data)

    def validCoord(self, x, y):
        return (x >= 0 and x < self.w and y >= 0 and y < self.h)

    # this must be implemented, because it's called from C++
    def walkable(self, x, y):
        return (self.validCoord(x, y) and self.data[y][x] != ord("#"))

    def __str__(self):
        tmprepr = []
        repr = ""

        for line in data:
            tmprepr.append("".join(map(chr, line)))

        repr = "\n".join(tmprepr)

        return repr

    def findpath(self, start, goal):
        return astarkikkare.search(self, start, goal)

    def print_with_path(self, start, goal):
        path = self.findpath(start, goal)

        for y in range(len(self.data)):
            for x in range(len(self.data[0])):
                if (x, y) in path:
                    print("x", sep="", end="")
                else:
                    print(chr(self.data[y][x]), sep="", end="")

            print()


a = GameMap("astar-test.map")
a.print_with_path((19,1), (1, 10))