AStar寻路

#include "pch.h"
#include <iostream>
#include <vector>
#include <windows.h>
using namespace std;
//行 高
#define ROW 12
//列 宽
#define COL 12

#define ZXDJ 10
#define XXDJ 14

struct MyPoint
{
    int row;
    int col;
    //g起点到当前点代价
    //w权重
    //h当前点到终点的预估代价 忽略障碍只看直线
    //f = g+h+w
    int g, h, f;
    void setF() { f = g + h; }
};


//八个方向
enum direct {
    p_up,p_down,p_left,p_right,p_lup,p_ldown,p_rup,p_rdown
};


//辅助地图节点
//记录走过没，代价值
struct pathNode
{
    int val;//值
    bool isFind;//走过没，true走过

};

//判断pos点能不能走,能true
bool canWalk(MyPoint pos, pathNode pathMap[ROW][COL]) {
    //不在地图内
    if (pos.row < 0||pos.row>=ROW||pos.col<0||pos.col>=COL) {
    return false;}
    //是障碍，不能走
    if (pathMap[pos.row][pos.col].val == 1)return false;
    //走过了
    if (pathMap[pos.row][pos.col].isFind == 1)return false;
    return true;
}


//树的节点类型,使用树存储走过路径
struct treeNode {
    MyPoint pos;
    treeNode* pParent;//指向父节点指针
    vector<treeNode*> child;//保存所有孩子节点数组
};



//计算g值
int getG(MyPoint pos,MyPoint endPos);


int main()
{
    //地图 0：路  1：障碍  2:大楼 3：井盖
    int map[ROW][COL] = {
        {0,0,0,0,0,1,0,0,0,0,0,0},
        {0,0,0,0,0,1,0,0,0,0,0,0},
        {0,0,0,0,0,1,0,0,0,0,0,0},
        {0,0,0,0,0,1,0,0,0,0,0,0},
        {0,0,0,0,0,1,0,0,0,0,0,0},
        {0,0,0,0,0,1,0,0,0,0,0,0},
        {0,0,0,0,0,1,0,0,0,0,0,0},
        {0,0,0,0,0,1,0,0,0,0,0,0},
        {0,0,0,0,0,1,0,0,0,0,0,0},
        {0,0,0,0,0,1,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,1,0,0,0,0,0,0},
    };
    //2 起点和重点
    MyPoint begPos = {1,1};
    MyPoint endPos = { 5,10 };
    //3 记录，辅助地图,初始化
    pathNode pathMap[ROW][COL] = { 0 };
    for (int i = 0;i<ROW;i++)
        for (int j = 0; j < COL; j++)
        {
            pathMap[i][j].val = map[i][j];
        }

    //4 创建一棵树
    treeNode* pRoot = NULL;

    //5 标记起点点走过
    pathMap[begPos.row][begPos.col].isFind = true;

    //6 起点入数成为根节点
    pRoot = new treeNode;
    memset(pRoot, 0, sizeof treeNode);//清空
    pRoot->pos = begPos;

    //7 准备一个数组保存所有评估点
    vector<treeNode*> buff;
    //迭代器找最小点
    vector<treeNode*>::iterator itMin;
    vector<treeNode*>::iterator it;
    //8 寻路
    bool isFindEnd = false;
    //当前点
    MyPoint currentPos = begPos;
    //当前树节点
    treeNode* pTemp = pRoot;
    while (true)
    {
        //8.1 检查当前点哪些需要计算
        for (int i = 0; i < 8; i++) {
            treeNode* pChild = new treeNode;
            memset(pChild, 0, sizeof treeNode);
            pChild->pos = pTemp->pos;
            switch (i)
            {
            case p_up:
                pChild->pos.row--;
                pChild->pos.g = 10;
                break;
            case p_down:
                pChild->pos.row++;
                pChild->pos.g = 10;
                break;
            case p_left:
                pChild->pos.col--;
                pChild->pos.g = 10;
                break;
            case p_right:
                pChild->pos.col++;
                pChild->pos.g = ZXDJ;
                break;
            case p_lup:
                pChild->pos.col--;
                pChild->pos.row--;
                pChild->pos.g = XXDJ;
                break;
            case p_ldown:
                pChild->pos.col--;
                pChild->pos.row++;
                pChild->pos.g = XXDJ;
                break;
            case p_rup:
                pChild->pos.col++;
                pChild->pos.row--;
                pChild->pos.g = XXDJ;
                break;
            case p_rdown:
                pChild->pos.col++;
                pChild->pos.row++;
                pChild->pos.g = XXDJ;
                break;
            default:
                break;
            }//8.2 计算所有点f
            //8.2.1 判断是否计算(能走不)
            if (canWalk(pChild->pos,pathMap)) {
            //8.2.2 jisuanf
                pChild->pos.g = getG(pChild->pos, endPos);
                pChild->pos.setF();
                //8.2.3 判断能否入树buff
                pTemp->child.push_back(pChild);
                pChild->pParent = pTemp;
                buff.push_back(pChild);
                //printf("(%d,%d)", pChild->pos.row, pChild->pos.col);
            }
            

        }
        //cout << endl;
        //Sleep(2000);
        //8.3 找出f值最小的点，走，并从buff中删除该店
        itMin = buff.begin();
        for (it = buff.begin(); it != buff.end(); it++) {
            if ((*itMin)->pos.f > (*it)->pos.f) {
                itMin = it;
            }
        }

        pathMap[pTemp->pos.row][pTemp->pos.col].isFind = true;
        pTemp = *itMin;//走
        buff.erase(itMin);
        //8.4 判断是否找到重点，判断整个地图是否没有终点
        if (pTemp->pos.row == endPos.row&&
            pTemp->pos.col == endPos.col) {
            isFindEnd = true;
            break;
        }
        if (buff.empty()) { break; }
    }
    //9 打印路径
    if (isFindEnd) {
        printf("找到终点了！\n");
        while (pTemp) {
            printf("(%d,%d)", pTemp->pos.row, pTemp->pos.col);
            pTemp = pTemp->pParent;
        }
        cout << "\n";
    }

    cout << fun();
    system("pause");
}



int getG(MyPoint pos, MyPoint endPos) {
    int x = ((pos.col > endPos.col) ? (pos.col - endPos.col) : (endPos.col - pos.col));
    int y = ((pos.row > endPos.row) ? (pos.row - endPos.row) : (endPos.row - pos.row));
    return ZXDJ * (x + y);
}
// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单
// 调试程序: F5 或调试 >“开始调试”菜单

// 入门提示: 
//   1. 使用解决方案资源管理器窗口添加/管理文件
//   2. 使用团队资源管理器窗口连接到源代码管理
//   3. 使用输出窗口查看生成输出和其他消息
//   4. 使用错误列表窗口查看错误
//   5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目
//   6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件