A*算法C/C++代码

A_star.h

#pragma once

#include "stdafx.h"
#include<iostream>
#include<math.h>
using namespace std;

#define for_manhattan 10//为h加权，增强启发搜索
#define map_row 50
#define map_col 50

typedef struct Map{
    char s_ch;
    int s_x, s_y;
    double s_f,s_h, s_g;
    bool s_visit;
    Map *s_pre;
    Map(){
        s_visit = 0;
        s_pre = NULL;
    }
}Astar_Node;

typedef struct ListNode{
    Astar_Node astar;
    struct ListNode *next;
}LinkList;

extern LinkList *OpenList;
extern Astar_Node Astar_map[map_row][map_col], Astar_start, Astar_end;


int judge(int x, int y);
int max(int a, int b);
double Manhattan_dis(int sx, int sy, int ex, int ey);
double Euclidean_dis(int sx, int sy, int ex, int ey);
double Chebyshev_dis(int sx, int sy, int ex, int ey);
double jiaquan_Manhattan(int sx, int sy, int ex, int ey);

void Init_list(LinkList *&L);
void destroy_List(LinkList *L);
bool List_empty(LinkList *L);
void insert_Openlist(LinkList *&L, Astar_Node x);
void delete_Openlist(LinkList *&L);

int Astar_Process();
int Astar_viewback();

A_star.cpp

#include "stdafx.h"
#include "A_star.h"
//#include"LINK_List.h"

LinkList *OpenList;
Astar_Node Astar_map[map_row][map_col], Astar_start, Astar_end;

int judge(int x, int y){
    if (x >= 0 && x<map_col && y >= 0 && y<map_row)
        return 1;
    return 0;
}

int max(int a, int b){
    return a>b ? a : b;
}

double Manhattan_dis(int sx, int sy, int ex, int ey){//曼哈顿
    double nn;
    nn = abs(sx - ex) + abs(sy - ey);
    nn *= for_manhattan;
    return nn;
}

double Euclidean_dis(int sx, int sy, int ex, int ey){//欧几里德
    double nn;
    nn = sqrt((sx - ex)*(sx - ex) + (sy - ey)*(sy - ey));
    return nn;
}

double Chebyshev_dis(int sx, int sy, int ex, int ey){//切比雪夫
    double nn;
    nn = max(abs(sx - ex), abs(sy - ey));
    return nn;
}

double jiaquan_Manhattan(int sx, int sy, int ex, int ey){//加权曼哈顿   //better
    double nn, dx, dy;
    dx = abs(sx - ex);
    dy = abs(sy - ey);
    if (dx>dy)
        nn = 10 * dx + 6 * dy;
    else
        nn = 6 * dx + 10 * dy;
    return nn;
}
void Init_list(LinkList *&L)
{
    L = (LinkList *)malloc(sizeof(LinkList));
    L->next = nullptr;
}
void destroy_List(LinkList *L)
{
    LinkList*pre = L, *p = L->next;
    if (p != nullptr)
    {
        free(pre);
        pre = p;
        p = pre->next;
    }
    free(pre);
}
bool List_empty(LinkList *L)
{
    return(L->next == nullptr);
}
void insert_Openlist(LinkList *&L, Astar_Node x)
{
    LinkList* LL = (LinkList *)malloc(sizeof(LinkList));
    LL->astar = x;
    LL->next = nullptr;

    LinkList*p = L;
    while (p->next != nullptr&&p->next->astar.s_f < LL->astar.s_f)
    {
        p = p->next;
    }

    LL->next = p->next;
    p->next = LL;

}

void delete_Openlist(LinkList *&L)
{
    L->next = L->next->next;
}

int dir[4][2] = { { 1, 0 }, { 0, 1 }, { -1, 0 }, { 0, -1 } };//4个方向
//int dir[8][2] = { { 0, -1 }, { 1, -1 }, { 1, 0 }, { 1, 1 }, { 0, 1 }, { -1, 1 }, { -1, 0 }, { -1, -1 } };//8个方向

int Astar_Process()
{
    Init_list(OpenList);
    insert_Openlist(OpenList, Astar_start);

    Astar_Node now;
    int xx, yy;
    int i;

    while (!List_empty(OpenList)){
        //now = q.top();
        now = OpenList->next->astar;
        cout << now.s_x << " " << now.s_y << endl;
        //q.pop();
        delete_Openlist(OpenList);
        if (now.s_x == Astar_end.s_x && now.s_y == Astar_end.s_y)
            break;
        for (i = 0; i<4; i++){
            xx = now.s_x + dir[i][0];
            yy = now.s_y + dir[i][1];
            if (!judge(xx, yy) || Astar_map[xx][yy].s_ch == 'x' || Astar_map[xx][yy].s_visit == 1)
                continue;

            Astar_map[xx][yy].s_pre = &Astar_map[now.s_x][now.s_y];
            Astar_map[xx][yy].s_visit = 1;

            Astar_map[xx][yy].s_h = Manhattan_dis(xx, yy, Astar_end.s_x, Astar_end.s_y);//选择想用的h(x)
            Astar_map[xx][yy].s_g = now.s_g + 1;
            Astar_map[xx][yy].s_f = Astar_map[xx][yy].s_g + Astar_map[xx][yy].s_h;

            cout << xx << " " << yy << " " << Astar_map[xx][yy].s_g << " " << Astar_map[xx][yy].s_h << " " << Astar_map[xx][yy].s_f << endl;

            //q.push(map[xx][yy]);
            insert_Openlist(OpenList, Astar_map[xx][yy]);
        }
        cout << endl;
    }
    return 0;
}

int Astar_viewback(){
    Astar_Node *p;
    int path_length = 0;
    p = Astar_map[Astar_end.s_x][Astar_end.s_y].s_pre;

    cout << "Path:" << endl;
    while (p){
        if (p->s_pre)
            p->s_ch = 'v';

        cout << p->s_x << "  " << p->s_y << endl;

        p = p->s_pre;
        path_length++;

    }
    cout << "Path_length:"<<path_length << endl;
    return 0;
}

// A_starPath.cpp : 定义控制台应用程序的入口点。
//

#include "stdafx.h"
#include<iostream>
#include<math.h>
#include<fstream>
#include "A_star.h"
using namespace std;

fstream file_data;

int main()
{
    Init_list(OpenList);
    file_data.open("test_data1.txt", ios::in);
    int i, j, visi_num = 0;
    for (i = 0; i<map_row; i++){
        for (j = 0; j<map_col; j++){
            file_data >> Astar_map[i][j].s_ch;
            cout << Astar_map[i][j].s_ch;
            Astar_map[i][j].s_x = i;
            Astar_map[i][j].s_y = j;
            if (Astar_map[i][j].s_ch == 's')
            {
                Astar_map[i][j].s_g = 0;
                Astar_map[i][j].s_f = 0;
                Astar_map[i][j].s_visit = 1;
                Astar_start = Astar_map[i][j];
            }

            if (Astar_map[i][j].s_ch == 'e')
            {
                Astar_end = Astar_map[i][j];
            }

        }
        cout << endl;
    }
    cout << "结果---------------------------------------------------" << endl;
    Astar_Process();
    Astar_viewback();

    for (i = 0; i<map_row; i++){
        for (j = 0; j<map_col; j++){
            visi_num += Astar_map[i][j].s_visit;
            if (Astar_map[i][j].s_ch == 'v' || Astar_map[i][j].s_ch == 'e' || Astar_map[i][j].s_ch == 's')
                printf("%c", Astar_map[i][j].s_ch);
            else if (Astar_map[i][j].s_visit)
                printf("%d", Astar_map[i][j].s_visit);
            else
                printf("%c", Astar_map[i][j].s_ch);
        }
        cout << "\n";
    }
    printf("\n遍历节点 %d 个\n", visi_num);

    destroy_List(OpenList);
    system("Pause");
    return 0;
}