编程练习:走迷宫问题后续

  上一篇文章中提到了走迷宫问题《编程练习:走迷宫问》题 是使用广度优先算法解出的,最后给出了A*算法,此算法是一种静态路网中求解最短路最有效的算法。最后扯点别的:在人工智能,游戏制作中好像很有用,对于参考文献《A* Pathfinding for Beginners》 中最后讨论的例如游戏设计中避免碰撞,不同地形损耗,路径平滑等,联想到“三国志11”中的操作,感觉醍醐灌顶,有时间参与此类工作要再多研究了。这篇文章主要介绍相应的实现。

问题描述:

  实现给定迷宫,给出你认为最快的走出迷宫步数和策略;若不存在,请输出0。

解决思路:

  A* (A-Star)算法是一种静态路网中求解最短路最有效的直接搜索方法,也是许多其他问题的常用启发式算法。
注意是最有效的直接搜索算法。之后涌现了很多预处理算法(ALT,CH,HL等等),在线查询效率是A*算法的数千甚至上万倍。
  公式表示为: f(n)=g(n)+h(n),
  其中 f(n) 是从初始状态经由状态n到目标状态的代价估计,g(n) 是在状态空间中从初始状态到状态n的实际代价,h(n) 是从状态n到目标状态的最佳路径的估计代价。(对于路径搜索问题,状态就是图中的节点,代价就是距离)百度百科《A*算法》
  上述链接还给出了可运行的C语言程序(已运行没问题),如果做嵌入式设计是否能修改移植呢?嗯。。。


代码实现:

  此处尝试编写A*算法,但是google发现了类似实现,现记录如下:

/*
//转自:http://baike.baidu.com/link?url=j_pHMECOXPa_eidbnJhdVPCQDoKsk1EjtEkPhsfXRG0xCGSCyoTNOIVWZrBt5NuLmzVgZPzAczicC9rRJBSsn0u8x_0cDSBtbUzCSvBhsW7
#include <stdio.h>
#include <math.h>
#define _CRT_SECURE_NO_WARNINGS

#define MaxLength 100    //用于优先队列(Open表)的数组
#define Height     15    //地图高度
#define Width      20    //地图宽度

#define Reachable   0    //可以到达的结点
#define Bar         1    //障碍物
#define Pass        2    //需要走的步数
#define Source      3    //起点
#define Destination 4    //终点

#define Sequential  0    //顺序遍历
#define NoSolution  2    //无解决方案
#define Infinity    0xfffffff

#define East       (1 << 0)
#define South_East (1 << 1)
#define South      (1 << 2)
#define South_West (1 << 3)
#define West       (1 << 4)
#define North_West (1 << 5)
#define North      (1 << 6)
#define North_East (1 << 7)

typedef struct
{
    signed char x, y;
} Point;

const Point dir[8] =
{
    { 0, 1 },   // East
    { 1, 1 },   // South_East
    { 1, 0 },   // South
    { 1, -1 },  // South_West
    { 0, -1 },  // West
    { -1, -1 }, // North_West
    { -1, 0 },  // North
    { -1, 1 }   // North_East
};

unsigned char within(int x, int y)
{
    return (x >= 0 && y >= 0
        && x < Height && y < Width);
}

typedef struct
{
    int x, y;
    unsigned char reachable, sur, value;
} MapNode;

typedef struct Close
{
    MapNode *cur;
    char vis;
    struct Close *from;
    float F, G;
    int H;
} Close;

typedef struct //优先队列(Open表)
{
    int length;        //当前队列的长度
    Close* Array[MaxLength];    //评价结点的指针
} Open;

static MapNode graph[Height][Width];
static int srcX, srcY, dstX, dstY;    //起始点、终点
static Close close[Height][Width];

// 优先队列基本操作
void initOpen(Open *q)    //优先队列初始化
{
    q->length = 0;        // 队内元素数初始为0
}

void push(Open *q, Close cls[Height][Width], int x, int y, float g)
{    //向优先队列(Open表)中添加元素
    Close *t;
    int i, mintag;
    cls[x][y].G = g;    //所添加节点的坐标
    cls[x][y].F = cls[x][y].G + cls[x][y].H;
    q->Array[q->length++] = &(cls[x][y]);
    mintag = q->length - 1;
    for (i = 0; i < q->length - 1; i++)
    {
        if (q->Array[i]->F < q->Array[mintag]->F)
        {
            mintag = i;
        }
    }
    t = q->Array[q->length - 1];
    q->Array[q->length - 1] = q->Array[mintag];
    q->Array[mintag] = t;    //将评价函数值最小节点置于队头
}

Close* shift(Open *q)
{
    return q->Array[--q->length];
}

// 地图初始化操作
void initClose(Close cls[Height][Width], int sx, int sy, int dx, int dy)
{    // 地图Close表初始化配置
    int i, j;
    for (i = 0; i < Height; i++)
    {
        for (j = 0; j < Width; j++)
        {
            cls[i][j].cur = &graph[i][j];        // Close表所指节点
            cls[i][j].vis = !graph[i][j].reachable;        // 是否被访问
            cls[i][j].from = NULL;                // 所来节点
            cls[i][j].G = cls[i][j].F = 0;
            cls[i][j].H = abs(dx - i) + abs(dy - j);    // 评价函数值
        }
    }
    cls[sx][sy].F = cls[sx][sy].H;            //起始点评价初始值
    //    cls[sy][sy].G = 0;                        //移步花费代价值
    cls[dx][dy].G = Infinity;
}

void initGraph(const int map[Height][Width], int sx, int sy, int dx, int dy)
{    //地图发生变化时重新构造地
    int i, j;
    srcX = sx;    //起点X坐标
    srcY = sy;    //起点Y坐标
    dstX = dx;    //终点X坐标
    dstY = dy;    //终点Y坐标
    for (i = 0; i < Height; i++)
    {
        for (j = 0; j < Width; j++)
        {
            graph[i][j].x = i; //地图坐标X
            graph[i][j].y = j; //地图坐标Y
            graph[i][j].value = map[i][j];
            graph[i][j].reachable = (graph[i][j].value == Reachable);    // 节点可到达性
            graph[i][j].sur = 0; //邻接节点个数
            if (!graph[i][j].reachable)
            {
                continue;
            }
            if (j > 0)
            {
                if (graph[i][j - 1].reachable)    // left节点可以到达
                {
                    graph[i][j].sur |= West;
                    graph[i][j - 1].sur |= East;
                }
                if (i > 0)
                {
                    if (graph[i - 1][j - 1].reachable
                        && graph[i - 1][j].reachable
                        && graph[i][j - 1].reachable)    // up-left节点可以到达
                    {
                        graph[i][j].sur |= North_West;
                        graph[i - 1][j - 1].sur |= South_East;
                    }
                }
            }
            if (i > 0)
            {
                if (graph[i - 1][j].reachable)    // up节点可以到达
                {
                    graph[i][j].sur |= North;
                    graph[i - 1][j].sur |= South;
                }
                if (j < Width - 1)
                {
                    if (graph[i - 1][j + 1].reachable
                        && graph[i - 1][j].reachable
                        && map[i][j + 1] == Reachable) // up-right节点可以到达
                    {
                        graph[i][j].sur |= North_East;
                        graph[i - 1][j + 1].sur |= South_West;
                    }
                }
            }
        }
    }
}

int bfs()
{
    int times = 0;
    int i, curX, curY, surX, surY;
    unsigned char f = 0, r = 1;
    Close *p;
    Close* q[MaxLength] = { &close[srcX][srcY] };

    initClose(close, srcX, srcY, dstX, dstY);
    close[srcX][srcY].vis = 1;

    while (r != f)
    {
        p = q[f];
        f = (f + 1) % MaxLength;
        curX = p->cur->x;
        curY = p->cur->y;
        for (i = 0; i < 8; i++)
        {
            if (!(p->cur->sur & (1 << i)))
            {
                continue;
            }
            surX = curX + dir[i].x;
            surY = curY + dir[i].y;
            if (!close[surX][surY].vis)
            {
                close[surX][surY].from = p;
                close[surX][surY].vis = 1;
                close[surX][surY].G = p->G + 1;
                q[r] = &close[surX][surY];
                r = (r + 1) % MaxLength;
            }
        }
        times++;
    }
    return times;
}

int astar()
{    // A*算法遍历
    //int times = 0;
    int i, curX, curY, surX, surY;
    float surG;
    Open q; //Open表
    Close *p;

    initOpen(&q);
    initClose(close, srcX, srcY, dstX, dstY);
    close[srcX][srcY].vis = 1;
    push(&q, close, srcX, srcY, 0);

    while (q.length)
    {    //times++;
        p = shift(&q);
        curX = p->cur->x;
        curY = p->cur->y;
        if (!p->H)
        {
            return Sequential;
        }
        for (i = 0; i < 8; i++)
        {
            if (!(p->cur->sur & (1 << i)))
            {
                continue;
            }
            surX = curX + dir[i].x;
            surY = curY + dir[i].y;
            if (!close[surX][surY].vis)
            {
                close[surX][surY].vis = 1;
                close[surX][surY].from = p;
                surG = p->G + sqrt((double)((curX - surX) * (curX - surX) + (curY - surY) * (curY - surY)));
                push(&q, close, surX, surY, surG);
            }
        }
    }
    //printf("times: %d\n", times);
    return NoSolution; //无结果
}

const int map[Height][Width] = {
    { 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1 },
    { 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1 },
    { 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 1 },
    { 0, 0, 0, 0, 0, 1, 0, 1, 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, 1, 0, 1 },
    { 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, 1, 0, 0, 0, 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, 1, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 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, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0 },
    { 0, 0, 0, 0, 1, 0, 0, 1, 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, 0, 1, 1, 0 },
    { 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 1 },
    { 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 }
};

const char Symbol[5][3] = { "□", "▓", "▽", "☆", "◎" };

void printMap()
{
    int i, j;
    for (i = 0; i < Height; i++)
    {
        for (j = 0; j < Width; j++)
        {
            printf("%s", Symbol[graph[i][j].value]);
        }
        puts("");
    }
    puts("");
}

Close* getShortest()
{    // 获取最短路径
    int result = astar();
    Close *p, *t, *q = NULL;
    switch (result)
    {
    case Sequential:    //顺序最近
        p = &(close[dstX][dstY]);
        while (p)    //转置路径
        {
            t = p->from;
            p->from = q;
            q = p;
            p = t;
        }
        close[srcX][srcY].from = q->from;
        return &(close[srcX][srcY]);
    case NoSolution:
        return NULL;
    }
    return NULL;
}

static Close *start;
static int shortestep;
int printShortest()
{
    Close *p;
    int step = 0;

    p = getShortest();
    start = p;
    if (!p)
    {
        return 0;
    }
    else
    {
        while (p->from)
        {
            graph[p->cur->x][p->cur->y].value = Pass;
            printf("(%d%d)→\n", p->cur->x, p->cur->y);
            p = p->from;
            step++;
        }
        printf("(%d%d)\n", p->cur->x, p->cur->y);
        graph[srcX][srcY].value = Source;
        graph[dstX][dstY].value = Destination;
        return step;
    }
}

void clearMap()
{    // Clear Map Marks of Steps
    Close *p = start;
    while (p)
    {
        graph[p->cur->x][p->cur->y].value = Reachable;
        p = p->from;
    }
    graph[srcX][srcY].value = map[srcX][srcY];
    graph[dstX][dstY].value = map[dstX][dstY];
}

void printDepth()
{
    int i, j;
    for (i = 0; i < Height; i++)
    {
        for (j = 0; j < Width; j++)
        {
            if (map[i][j])
            {
                printf("%s ", Symbol[graph[i][j].value]);
            }
            else
            {
                printf("%2.0lf ", close[i][j].G);
            }
        }
        puts("");
    }
    puts("");
}

void printSur()
{
    int i, j;
    for (i = 0; i < Height; i++)
    {
        for (j = 0; j < Width; j++)
        {
            printf("%02x ", graph[i][j].sur);
        }
        puts("");
    }
    puts("");
}

void printH()
{
    int i, j;
    for (i = 0; i < Height; i++)
    {
        for (j = 0; j < Width; j++)
        {
            printf("%02d ", close[i][j].H);
        }
        puts("");
    }
    puts("");
}

int main(int argc, const char **argv)
{
    initGraph(map, 0, 0, 0, 0);
    printMap();

    while (scanf("%d %d %d %d", &srcX, &srcY, &dstX, &dstY) != EOF)
    {
        if (within(srcX, srcY) && within(dstX, dstY))
        {
            if (shortestep = printShortest())
            {
                printf("从(%d%d)到(%d%d)的最短步数是: %d\n",
                    srcX, srcY, dstX, dstY, shortestep);
                printMap();
                clearMap();
                bfs();
                printDepth();
                puts((shortestep == close[dstX][dstY].G) ? "正确" : "错误");
                clearMap();
            }
            else
            {
                printf("从(%d%d)不可到达(%d%d)\n",
                    srcX, srcY, dstX, dstY);
            }
        }
        else
        {
            puts("输入错误!");
        }
    }
    return (0);
}*/


//中文注释自己添加的理解,详细学习
//英文版:http://www.gamedev.net/page/resources/_/technical/artificial-intelligence/a-pathfinding-for-beginners-r2003
//中文翻译版:http://zhyt710.iteye.com/blog/739803
#include <iostream>
#include <iomanip>
#include <queue>
#include <string>
#include <math.h>
#include <ctime>
using namespace std;

//定义地图尺寸
const int n = 60; // horizontal size of the map
const int m = 60; // vertical size size of the map
static int map[n][m];
//定义开,闭二维表
static int closed_nodes_map[n][m]; // map of closed (tried-out) nodes
static int open_nodes_map[n][m]; // map of open (not-yet-tried) nodes
static int dir_map[n][m]; // map of directions
//定义邻域大小,默认四邻域
const int dir = 8; // number of possible directions to go at any position
// if dir==4
//static int dx[dir]={1, 0, -1, 0};
//static int dy[dir]={0, 1, 0, -1};
// if dir==8
static int dx[dir] = { 1, 1, 0, -1, -1, -1, 0, 1 };
static int dy[dir] = { 0, 1, 1, 1, 0, -1, -1, -1 };


//节点类
class node
{
    // current position
    int xPos;
    int yPos;
    // total distance already travelled to reach the node
    //此处的level相当于参考文献中的G,表示地理距离,priority相当于F,即F=G+H,H是不考虑不可达区域的待测点到终点距离函数度量,此处有,欧氏距离,街区距离,切比雪夫距离。
    int level;
    // priority=level+remaining distance estimate
    int priority;  // smaller: higher priority

public:
    node(int xp, int yp, int d, int p)
    {
        xPos = xp; yPos = yp; level = d; priority = p;
    }
    //获取位置及各式权重方法
    int getxPos() const { return xPos; }
    int getyPos() const { return yPos; }
    int getLevel() const { return level; }
    int getPriority() const { return priority; }
    //更新权重方法
    void updatePriority(const int & xDest, const int & yDest)
    {
        priority = level + estimate(xDest, yDest) * 10; //A*
    }

    // give better priority to going strait instead of diagonally
    //注意此处的方向标号,考虑8邻域方向,垂直向下为2,顺时针方向标记
    //|5|6|7|
    //|4| |8|
    //|3|2|1|
    //所有水平垂直方向测度均为10,对角线距离测度为14sqrt(2)*10)
    void nextLevel(const int & i) // i: direction
    {
        level += (dir == 8 ? (i % 2 == 0 ? 10 : 14) : 10);
    }

    // Estimation function for the remaining distance to the goal.
    const int & estimate(const int & xDest, const int & yDest) const
    {
        static int xd, yd, d;
        xd = xDest - xPos;
        yd = yDest - yPos;

        // Euclidian Distance
        //d = static_cast<int>(sqrt(xd*xd + yd*yd));

        // Manhattan distance
        d=abs(xd)+abs(yd);

        // Chebyshev distance
        //d=max(abs(xd), abs(yd));

        return(d);
    }
};

//为了算法中比较新考虑节点与open list中节点那个距离更近
// Determine priority (in the priority queue)
bool operator<(const node & a, const node & b)
{
    return a.getPriority() > b.getPriority();
}

// A-star algorithm.
// The route returned is a string of direction digits.
string pathFind(const int & xStart, const int & yStart,
    const int & xFinish, const int & yFinish)
{
    //此处作者开辟了两个优先队列,用于处理当新增节点不如open list中继续寻路情况,需要改动父节点并删除,并入close list中
    static priority_queue<node> pq[2]; // list of open (not-yet-tried) nodes
    static int pqi; // pq index
    static node* n0;
    static node* m0;
    static int i, j, x, y, xdx, ydy;
    static char c;
    pqi = 0;

    //初始化
    // reset the node maps
    for (y = 0; y<m; y++)
    {
        for (x = 0; x<n; x++)
        {
            closed_nodes_map[x][y] = 0;
            open_nodes_map[x][y] = 0;
        }
    }

    // create the start node and push into list of open nodes
    //开始点的位置,距离度量初始化
    n0 = new node(xStart, yStart, 0, 0);
    //计算F=G+H中的H(理想情况下移动到终点的耗费)即可
    n0->updatePriority(xFinish, yFinish);
    pq[pqi].push(*n0);
    open_nodes_map[x][y] = n0->getPriority(); // mark it on the open nodes map

    // A* search
    while (!pq[pqi].empty())
    {
        // get the current node w/ the highest priority
        // from the list of open nodes
        n0 = new node(pq[pqi].top().getxPos(), pq[pqi].top().getyPos(),
            pq[pqi].top().getLevel(), pq[pqi].top().getPriority());

        x = n0->getxPos(); y = n0->getyPos();

        pq[pqi].pop(); // remove the node from the open list
        open_nodes_map[x][y] = 0;
        // mark it on the closed nodes map
        closed_nodes_map[x][y] = 1;

        // quit searching when the goal state is reached
        //if((*n0).estimate(xFinish, yFinish) == 0)
        //首先考虑了当寻找到终点并输出策略的情况
        if (x == xFinish && y == yFinish)
        {
            // generate the path from finish to start
            // by following the directions
            string path = "";
            while (!(x == xStart && y == yStart))
            {
                //此处将距离变量登记到dir_map中,当最终找到终点时,应用此方向记录可回溯至父节点
                j = dir_map[x][y];
                c = '0' + (j + dir / 2) % dir;
                path = c + path;
                x += dx[j];
                y += dy[j];
            }

            // garbage collection
            delete n0;
            // empty the leftover nodes
            //当已经找到终点时,后续open list都不需要考虑了
            while (!pq[pqi].empty()) pq[pqi].pop();
            return path;
        }

        // generate moves (child nodes) in all possible directions
        for (i = 0; i<dir; i++)
        {
            xdx = x + dx[i]; ydy = y + dy[i];
            //考虑8邻域方向,需要满足合法位置,且不再close list中,且原始地图中标记可行进时
            if (!(xdx<0 || xdx>n - 1 || ydy<0 || ydy>m - 1 || map[xdx][ydy] == 1
                || closed_nodes_map[xdx][ydy] == 1))
            {
                // generate a child node
                m0 = new node(xdx, ydy, n0->getLevel(),
                    n0->getPriority());
                //移动到邻域时耗费
                m0->nextLevel(i);
                //移动到终点时耗费
                //Ps,此处与参考文献中的计算方法不同,参考文献中H只计算一次存储就好,而这里每考虑一次就重新计算,速度必然慢,优化可考虑这里
                m0->updatePriority(xFinish, yFinish);

                // if it is not in the open list then add into that
                if (open_nodes_map[xdx][ydy] == 0)
                {
                    //若第一遍历到此
                    open_nodes_map[xdx][ydy] = m0->getPriority();
                    pq[pqi].push(*m0);
                    // mark its parent node direction
                    //更新方向记录表(参考文献是记录父指针,相应的记录空间会少,但这里也可以输出策略,可后续修改)
                    dir_map[xdx][ydy] = (i + dir / 2) % dir;
                }
                //若已经在open list中,需要考虑那种方式耗费更低
                else if (open_nodes_map[xdx][ydy]>m0->getPriority())
                {
                    //以下是当open list中耗费更低时修改父指针(异曲同工)
                    // update the priority info
                    open_nodes_map[xdx][ydy] = m0->getPriority();
                    // update the parent direction info
                    dir_map[xdx][ydy] = (i + dir / 2) % dir;

                    // replace the node
                    // by emptying one pq to the other one
                    // except the node to be replaced will be ignored
                    // and the new node will be pushed in instead
                    //修改后需要删除open list中相应的项,此处开辟了两个priority_queue,有两个就可以有空间操作了,其实完全可以用一个+erase完成吧,后需优化再说吧
                    while (!(pq[pqi].top().getxPos() == xdx &&
                        pq[pqi].top().getyPos() == ydy))
                    {
                        pq[1 - pqi].push(pq[pqi].top());
                        pq[pqi].pop();
                    }
                    pq[pqi].pop(); // remove the wanted node

                    //此处就是考虑了当移动后多于一般或少于一半时,通过交换指针减少操作次数
                    // empty the larger size pq to the smaller one
                    if (pq[pqi].size()>pq[1 - pqi].size()) pqi = 1 - pqi;
                    while (!pq[pqi].empty())
                    {
                        pq[1 - pqi].push(pq[pqi].top());
                        pq[pqi].pop();
                    }
                    pqi = 1 - pqi;
                    pq[pqi].push(*m0); // add the better node instead
                }
                else delete m0; // garbage collection
            }
        }
        delete n0; // garbage collection
    }
    return ""; // no route found
}

int main()
{
    srand(time(NULL));

    // create empty map
    for (int y = 0; y<m; y++)
    {
        for (int x = 0; x<n; x++) map[x][y] = 0;
    }

    // fillout the map matrix with a '+' pattern
    for (int x = n / 8; x<n * 7 / 8; x++)
    {
        map[x][m / 2] = 1;
    }
    for (int y = m / 8; y<m * 7 / 8; y++)
    {
        map[n / 2][y] = 1;
    }

    // randomly select start and finish locations
    int xA, yA, xB, yB;
    switch (rand() % 8)
    {
    case 0: xA = 0; yA = 0; xB = n - 1; yB = m - 1; break;
    case 1: xA = 0; yA = m - 1; xB = n - 1; yB = 0; break;
    case 2: xA = n / 2 - 1; yA = m / 2 - 1; xB = n / 2 + 1; yB = m / 2 + 1; break;
    case 3: xA = n / 2 - 1; yA = m / 2 + 1; xB = n / 2 + 1; yB = m / 2 - 1; break;
    case 4: xA = n / 2 - 1; yA = 0; xB = n / 2 + 1; yB = m - 1; break;
    case 5: xA = n / 2 + 1; yA = m - 1; xB = n / 2 - 1; yB = 0; break;
    case 6: xA = 0; yA = m / 2 - 1; xB = n - 1; yB = m / 2 + 1; break;
    case 7: xA = n - 1; yA = m / 2 + 1; xB = 0; yB = m / 2 - 1; break;
    }

    cout << "Map Size (X,Y): " << n << "," << m << endl;
    cout << "Start: " << xA << "," << yA << endl;
    cout << "Finish: " << xB << "," << yB << endl;
    // get the route
    clock_t start = clock();
    string route = pathFind(xA, yA, xB, yB);
    if (route == "") cout << "An empty route generated!" << endl;
    clock_t end = clock();
    double time_elapsed = double(end - start);
    cout << "Time to calculate the route (ms): " << time_elapsed << endl;
    cout << "Route:" << endl;
    cout << route << endl << endl;

    // follow the route on the map and display it 
    if (route.length()>0)
    {
        int j; char c;
        int x = xA;
        int y = yA;
        //对首尾及路径标记
        map[x][y] = 2;
        for (int i = 0; i<route.length(); i++)
        {
            c = route.at(i);
            j = atoi(&c);
            x = x + dx[j];
            y = y + dy[j];
            map[x][y] = 3;
        }
        map[x][y] = 4;
        //输出可视化美观
        // display the map with the route
        for (int y = 0; y<m; y++)
        {
            for (int x = 0; x<n; x++)
            if (map[x][y] == 0)
                cout << ".";
            else if (map[x][y] == 1)
                cout << "O"; //obstacle
            else if (map[x][y] == 2)
                cout << "S"; //start
            else if (map[x][y] == 3)
                cout << "R"; //route
            else if (map[x][y] == 4)
                cout << "F"; //finish
            cout << endl;
        }
    }
    getchar(); // wait for a (Enter) keypress  
    return(0);
}

相关参考网页:

《A* Pathfinding for Beginners》
《A*算法原理——面向初学者》
百度百科《A*算法》

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值