A* 算法實現

char g[24][81] = {
{"oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo"},
{"o o          o                                                                 o"},
{"o o          o                                                                 o"},
{"o                            oooooooooooooo                                    o"},
{"o o          o                            o                                    o"},
{"o o    s     o                            o                                    o"},
{"o o          o   ooooooo     oooooooooooooo       oooooooo                     o"},
{"o oooooooooooo   o     oooo  oo                   o      o                     o"},
{"o                o        o   oo                  ooo  ooo                     o"},
{"o                oooo  oooo    oo                                              o"},
{"o                              ooooooooooooooooooooooooooooooooooooooooooooooooo"},
{"o                                                                              o"},
{"o                                                                              o"},
{"o                                                                              o"},
{"oooooooooooooooooooooooooooooooooooooooooooo                                   o"},
{"o       o                                                           oooooooooooo"},
{"o       o   ooooooo                                      oooooooo              o"},
{"o       o         o                                      o      o              o"},
{"o       ooooooooooo        oooooooooo                    o  oo  o              o"},
{"o                          o        ooo                  o  o   o              o"},
{"o  e                       ooooo      o                  o  ooooo              o"},
{"o                                     o                  o                     o"},
{"o                              o      o                  o                     o"},
{"oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo"},
};

#include <stdio.h>
#include <assert.h>
#include <math.h>

typedef struct tagQUEUE QUEUE, *LPQUEUE;
struct tagQUEUE
{
    int tile;
    int f;
    int h;
    LPQUEUE next;
    LPQUEUE parent;
};

QUEUE queue[8192], h, t, f;
int xs, ys, xe, ye;
int r[8192];

#define TILE(x, y)  ((y) * 80 + x)
#define TILE_X(tile) ((tile) % 80)
#define TILE_Y(tile)    ((tile) / 80)

int
judge(int x, int y, int _h)
{
    y = ye - y;
    x = xe - x;
    return sqrt(x * x + y * y) * 10 + _h;
}

int
trytile(int x, int y, LPQUEUE parent, int _h)
{
    int tile;
    LPQUEUE s, loop, stop;

    tile = TILE(x, y);
    if (x < 0 || x >= 80 || y < 0 || y >= 24 || g[y][x] != ' ' || _h >= r[tile])
    {
        return -1;
    }
    r[tile] = _h;
    s = f.next;
    assert(s);
    s->tile = tile;
    f.next = s->next;
    s->parent = parent;
    s->h = _h;
    s->f = judge(x, y, _h);
    for (loop = &h; loop->f < s->f; loop = loop->next)
    {
        stop = loop;
    }
    s->next = stop->next;
    stop->next = s;

    return 0;
}

int
find_path()
{
    int i, x, y, tile, steps;
    LPQUEUE s;

    memset(queue, 0, sizeof(queue));
    memset(&f, 0, sizeof(f));
    f.next = queue;
    for (i = 0; i < sizeof(queue) / sizeof(queue[0]); ++i)
    {
        queue[i].next = queue + i + 1;
    }
    queue[sizeof(queue) / sizeof(queue[0]) - 1].next = 0;
    h.f = -1;
    h.next = &t;
    t.f = 0x7FFFFFFF;
    t.next = 0;
    for (i = 0; i < sizeof(r) / sizeof(r[0]); r[i++] = 0x7FFFFFFF);

    trytile(xs, ys, 0, 0);
    tile = TILE(xe, ye);

    for (;;)
    {
        s = h.next;
        if (s == &t)
        {
            return -1;
        }
        if (s->tile == tile)
        {
            break;
        }
        h.next = s->next;
        x = TILE_X(s->tile);
        y = TILE_Y(s->tile);
        trytile(x - 1, y, s, s->h + 10);
        trytile(x + 1, y, s, s->h + 10);
        trytile(x, y - 1, s, s->h + 10);
        trytile(x, y + 1, s, s->h + 10);
        trytile(x - 1, y - 1, s, s->h + 14);
        trytile(x - 1, y + 1, s, s->h + 14);
        trytile(x + 1, y - 1, s, s->h + 14);
        trytile(x + 1, y + 1, s, s->h + 14);
    }

    steps = 0;
    while (s)
    {
        x = TILE_X(s->tile);
        y = TILE_Y(s->tile);
        g[y][x] = '+';
        s = s->parent;
        ++steps;
    }
    printf("%d steps used./n", steps);
    g[ys][xs] = 's';
    g[ye][xe] = 'e';
    for (x = 0; x < 24; ++x)
    {
        printf("%s/n", g[x]);
    }

    return 0;
}

int
main(void)
{
    int i, j;

    for (i = 0; i < 24; i++)
    {
        for (j = 0; j < 80; ++j)
        {
            switch (g[i][j])
            {
                case 's':
                    xs = j;
                    ys = i;
                    g[i][j] = ' ';
                    break;
                case 'e':
                    xe = j;
                    ye = i;
                    g[i][j] = ' ';
                    break;
            }
        }
    }
    printf("<%d, %d> => <%d, %d>, %d bytes/n", xs, ys, xe, ye, sizeof(queue));
    find_path();
    return 0;
}