A*算法

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>


#define NODEEQUAL(a, b) (*((unsigned int *)(a)) == *((unsigned int *)(b)))
//#define GETDIST(a, b) ((abs((a)->x - (b)->x) + abs((a)->y - (b)->y)) * 10)
//#define GETDIST(a, b) ((int)(10 * sqrt((float)((a)->x - (b)->x) * ((a)->x - (b)->x) + ((a)->y - (b)->y) * ((a)->y - (b)->y))))

#define WORLDSIZEX 8
#define WORLDSIZEY 6
#define OBS ((char)1)



char g_world[WORLDSIZEY][WORLDSIZEX] =
{
    {0, 1, 0, 0, 0, 0, 0, 0},
    {0, 0, 1, 1, 1, 0, 0, 0},
    {0, 1, 1, 0, 1, 0, 0, 0},
    {0, 0, 0, 0, 1, 1, 1, 0},
    {0, 1, 1, 1, 0, 0, 1, 0},
    {0, 0, 0, 0, 1, 0, 0, 0}
};

typedef struct tag_loc
{
    short int x, y;
} loc_t, *loc_ptr;


typedef struct tag_node
{
    loc_t loc;
    int f, g;
    struct tag_node* father;
    struct tag_node* next;
} node_t, *node_ptr;


loc_t g_start = {0, 0};
loc_t g_end = {7, 4};
loc_t g_aroundpts[8] =
{
    { -1, -1}, { -1, 0}, { -1, 1}, {0, 1}, {1, 1}, {1, 0}, {1, -1}, {0, -1}
};


void print_path( node_ptr lst )
{
    if ( lst )
    {
        print_path( lst->father );
        fprintf( stdout, "(%d,%d) ", lst->loc.x, lst->loc.y );
    }
}

void list_destroy( node_ptr lst )
{
    node_ptr p;

    while ( lst )
    {
        p = lst->next;
        free( lst );
        lst = p;
    }
}

int get_dist( loc_ptr a, loc_ptr b )
{
    unsigned int x, y;

    x = abs( a->x - b->x );
    y = abs( a->y - b->y );

    if ( x == 0 )
    {
        return 10 * y;
    }

    if ( y == 0 )
    {
        return 10 * x;
    }

    return ( x + y ) * 7;
}

node_ptr create_closed( loc_ptr start, loc_ptr end )
{
    int h;
    node_ptr node;

    if ( start->x < 0 || start->x >= WORLDSIZEX || \
            start->y < 0 || start->y >= WORLDSIZEY || \
            end->x < 0 || end->x >= WORLDSIZEX || \
            end->y < 0 || end->y >= WORLDSIZEY )
    {
        fprintf( stderr, "起始或目标坐标非法!\n" );
        return NULL;
    }

    node = ( node_ptr )malloc( sizeof( node_t ) );
    if ( node == NULL )
    {
        fprintf( stderr, "malloc 内存不足!\n" );
        return NULL;
    }

    memset( node, 0, sizeof( node_t ) );
    memcpy( &node->loc, start, sizeof( loc_t ) );
    h = get_dist( &node->loc, end );
    //node->g = 0;
    node->f = node->g + h;
    //node->father = NULL;
    //node->next = NULL;

    return node;
}

static void _list_inst( node_ptr* header, node_ptr node )
{
    node_ptr h, p;

    h = *header;

    if ( h->f >= node->f )
    {
        node->next = h;
        *header = node;
    }
    else
    {
        p = h->next;
        while ( p )
        {
            if ( p->f >= node->f )
            {
                h->next = node;
                node->next = p;
                break;
            }
            h = p;
            p = p->next;
        }

        if ( p == NULL )
        {
            h->next = node;
            node->next = NULL;
        }
    }
}

static int _updt_opened( node_ptr* header, node_ptr node )
{
    node_ptr h, p, tmp;

    if ( *header == NULL )
    {
        tmp = ( node_ptr )malloc( sizeof( node_t ) );
        if ( tmp == NULL )
        {
            fprintf( stderr, "malloc 内存不足!\n" );
            return -1;
        }

        memcpy( tmp, node, sizeof( node_t ) );
        *header = tmp;
    }
    else
    {
        h = *header;
        while ( h )
        {
            if ( NODEEQUAL( h, node ) )
            {
                break;
            }
            p = h;
            h = h->next;
        }

        if ( h == NULL )
        {
            tmp = ( node_ptr )malloc( sizeof( node_t ) );
            if ( tmp == NULL )
            {
                fprintf( stderr, "malloc 内存不足!\n" );
                return -1;
            }

            memcpy( tmp, node, sizeof( node_t ) );
            _list_inst( header, tmp );
        }
        else
        {
            if ( h->g > node->g )
            {
                if ( h != *header )
                {
                    p->next = h->next;
                }
                else
                {
                    *header = h->next;
                }

                memcpy( h, node, sizeof( node_t ) );
                _list_inst( header, h );
            }
        }
    }

    return 0;
}

int insert_opened( node_ptr* opened, node_ptr closed, node_ptr father, int idx, loc_ptr end )
{
    int h;
    node_t node;

    // 先创建点
    memset( &node, 0, sizeof( node_t ) );

    node.loc.x = father->loc.x + g_aroundpts[idx].x;
    node.loc.y = father->loc.y + g_aroundpts[idx].y;

    if ( node.loc.x < 0 || node.loc.x >= WORLDSIZE || \
            node.loc.y < 0 || node.loc.y >= WORLDSIZE || \
            world[node.loc.y][node.loc.x] == 1 )
    {
        return 0;
    }

    if ( ( idx & 1 ) == 0 && \
            ( world[node.loc.y][father->loc.x] == 1 || \
              world[father->loc.y][node.loc.x] == 1 ) )
    {
        return 0;
    }

    node.g = ( ( idx & 1 ) ? 10 : 14 ) + father->g;
    h = get_dist( &node.loc, end );
    node.f = node.g + h;
    node.father = father;
    //node.next = NULL;

    // 该点是否在closed中
    while ( closed )
    {
        if ( NODEEQUAL( &node, closed ) )
        {
            return 0;
        }
        closed = closed->next;
    }


    // 该点不在closed中, 更新opened
    if ( _updt_opened( opened, &node ) == -1 )
    {
        return -1;
    }

    return 0;
}


int main()
{
    int i;
    node_ptr opened, closed;
    node_ptr curnode;
    //创建地图
    //.....................


    // 创建闭集
    if ( ( closed = create_closed( &g_start, &g_end ) ) == NULL )
    {
        return -1;
    }

    // 创建开集
    opened = NULL;


    // 起始点
    curnode = closed;

    // 运算
    while ( !NODEEQUAL( &curnode->loc, &g_end ) ) //终止条件
    {
        // 从当前点更新开集
        for ( i = 0; i < 8; i ++ )
        {
            if ( insert_opened( &opened, closed, curnode, i, &g_end ) == -1 )
            {
                goto err_exit;
            }
        }

        // 从开集中找到当前最小值, 并加入闭集
        if ( opened == NULL )
        {
            fprintf( stdout, "无路\n" );
            goto err_exit;
        }
        curnode->next = opened;
        opened = opened->next;
        curnode = curnode->next;
        curnode->next = NULL;
    }

    print_path( curnode );

    fprintf( stdout, "\ntotal = %d\n", curnode->g );

    list_destroy( opened );
    list_destroy( closed );

    return 0;
err_exit:
    list_destroy( opened );
    list_destroy( closed );
    return -1;
}

一个GUI演示程序ASTARGUI