A*算法,在最短路径的基础上,本人还想减少拐点应该怎么做?

.Net技术 码拜 7年前 (2015-11-12) 887次浏览
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
namespace AStarTest
{
    class AStarRoute
    {
        private int[][] map;  // 地图矩阵,0表示能通过,1表示不能通过
        private int map_w;    // 地图宽度
        private int map_h;    // 地图高度
        private int start_x;  // 起点坐标X
        private int start_y;  // 起点坐标Y
        private int goal_x;   // 终点坐标X
        private int goal_y;   // 终点坐标Y
        private bool[][] closeList;            // 关闭列表
        public int[][][] openList;               // 打开列表
        private int openListLength;
        private const int EXIST = 1;
        private const int NOT_EXIST = 0;
        private const int ISEXIST = 0;
        private const int EXPENSE = 1;     // 自身的代价
        private const int DISTANCE = 2;    // 距离的代价
        private const int COST = 3;        // 消耗的总代价
        private const int FATHER_DIR = 4;  // 父节点的方向
        public const int DIR_NULL = 0;
        public const int DIR_DOWN = 1;     // 方向:下
        public const int DIR_UP = 2;       // 方向:上
        public const int DIR_LEFT = 3;     // 方向:左
        public const int DIR_RIGHT = 4;    // 方向:右
        private int astar_counter;                // 算法嵌套深度
        private bool isFound;                  // 能否找到路径
        public AStarRoute(int[][] mx, int sx, int sy, int gx, int gy)
        {
            start_x = sx;
            start_y = sy;
            goal_x = gx;
            goal_y = gy;
            map = mx;
            map_w = mx.Length;
            map_h = mx[0].Length;
            astar_counter = 5000;
            initCloseList();
            initOpenList(goal_x, goal_y);
        }
        // 得到地图上这一点的消耗值
        private int getMapExpense(int x, int y)
        {
            return 1;
        }
        // 得到距离的消耗值
        private int getDistance(int x, int y, int ex, int ey)
        {
               return (Math.Abs(x - ex) + Math.Abs(y - ey));
        }
        // 得到给定坐标格子此时的总消耗值
        private int getCost(int x, int y)
        {
            return openList[x][y][COST];
        }
        // 开始寻路
        public void searchPath()
        {
            addOpenList(start_x, start_y);
            aStar(start_x, start_y);
        }
        // 寻路
        private void aStar(int x, int y)
        {
            // 控制算法深度
            for (int t = 0; t < astar_counter; t++)
            {
                if (((x == goal_x) && (y == goal_y)))
                {
                    isFound = true;
                    return;
                }
                else if ((openListLength == 0))
                {
                    isFound = false;
                    return;
                }
                removeOpenList(x, y);
                addCloseList(x, y);
                // 该点周围能够行走的点
                addNewOpenList(x, y, x, y + 1, DIR_UP);
                addNewOpenList(x, y, x, y - 1, DIR_DOWN);
                addNewOpenList(x, y, x - 1, y, DIR_RIGHT);
                addNewOpenList(x, y, x + 1, y, DIR_LEFT);
                // 找到估值最小的点,进行下一轮算法
                int cost = 0x7fffffff;
                //for (int i = 0; i < map_w; i++)
                for (int j = 0; j < map_h; j++)
                {
                   // for (int j = 0; j < map_h; j++)
                   for (int i = 0; i < map_w; i++)
                    {
                        if (openList[i][j][ISEXIST] == EXIST)
                        {
                            int newcost;
                                newcost = getCost(i, j);
                            if (openList[x][y][FATHER_DIR] != openList[i][j][FATHER_DIR])
                            {
                                 newcost = getCost(i, j) + 1;
                            }
                                 
                                if (cost > newcost)
                                {
                                 cost = newcost;
                                  x = i;
                                  y = j;
                                }
                            
                        }
                    }
                }
            }
            // 算法超深
            isFound = false;
            return;
        }
        // 添加一个新的节点
        private void addNewOpenList(int x, int y, int newX, int newY, int dir)
        {
            if (isCanPass(newX, newY))
            {
                if (openList[newX][newY][ISEXIST] == EXIST)
                {
                    if (openList[x][y][EXPENSE] + getMapExpense(newX, newY) <
                        openList[newX][newY][EXPENSE])
                    {
                        setFatherDir(newX, newY, dir);
                        setCost(newX, newY, x, y);
                    }
                }
                else
                {
                    addOpenList(newX, newY);
                    setFatherDir(newX, newY, dir);
                    setCost(newX, newY, x, y);
                }
            }
        }
        // 设置消耗值(子节点x,子节点y,父节点x,父节点y)
        private void setCost(int x, int y, int ex, int ey)
        {
            openList[x][y][EXPENSE] = openList[ex][ey][EXPENSE] + getMapExpense(x, y);
            openList[x][y][DISTANCE] = getDistance(x, y, ex, ey);
            openList[x][y][COST] = openList[x][y][EXPENSE] + openList[x][y][DISTANCE];
        }
        // 设置父节点方向
        private void setFatherDir(int x, int y, int dir)
        {
            openList[x][y][FATHER_DIR] = dir;
        }
        // 判断一个点能否可以通过
        private bool isCanPass(int x, int y)
        {
            // 超出边界
            if (x < 0 || x >= map_w || y < 0 || y >= map_h)
            {
                return false;
            }
            // 地图不通
            if (map[x][y] != 0)
            {
                return false;
            }
            // 在关闭列表中
            if (isInCloseList(x, y))
            {
                return false;
            }
            return true;
        }
        // 移除打开列表的一个元素
        private void removeOpenList(int x, int y)
        {
            if (openList[x][y][ISEXIST] == EXIST)
            {
                openList[x][y][ISEXIST] = NOT_EXIST;
                openListLength--;
            }
        }
        // 判断一点能否在关闭列表中
        private bool isInCloseList(int x, int y)
        {
            return closeList[x][y];
        }
        // 添加关闭列表
        private void addCloseList(int x, int y)
        {
            closeList[x][y] = true;
        }
        // 添加打开列表
        private void addOpenList(int x, int y)
        {
            if (openList[x][y][ISEXIST] == NOT_EXIST)
            {
                openList[x][y][ISEXIST] = EXIST;
                openListLength++;
            }
        }
        // 初始化关闭列表
        private void initCloseList()
        {
            closeList = new bool[map_w][];
            for (int k = 0; k < map_w; k++)
            {
                closeList[k] = new bool[map_h];
            }
            for (int i = 0; i < map_w; i++)
            {
                for (int j = 0; j < map_h; j++)
                {
                    closeList[i][j] = false;
                }
            }
        }
        // 初始化打开列表
        private void initOpenList(int ex, int ey)
        {
            openList = new int[map_h][][];
            for (int k = 0; k < map_h; k++)
            {
                openList[k] = new int[map_h][];
                for (int l = 0; l < map_h; l++)
                {
                    openList[k][l] = new int[5];
                }
            }
            for (int i = 0; i < map_w; i++)
            {
                for (int j = 0; j < map_h; j++)
                {
                    openList[i][j][ISEXIST] = NOT_EXIST;
                    openList[i][j][EXPENSE] = getMapExpense(i, j);
                    openList[i][j][DISTANCE] = getDistance(i, j, ex, ey);
                    openList[i][j][COST] = openList[i][j][EXPENSE] + openList[i][j][DISTANCE];
                    openList[i][j][FATHER_DIR] = DIR_NULL;
                }
            }
            openListLength = 0;
        }
        // 获得寻路结果
        public Result[] getResult()
        {
            Result[] result;
            List<Result> route;
            searchPath();
            if (!isFound)
            {
                return null;
            }
            route = new List<Result>();
            // openList是从目标点向起始点倒推的。
            int iX = goal_x;
            int iY = goal_y;
            while ((iX != start_x || iY != start_y))
            {
                route.Add(new Result(iX, iY));
                switch (Convert.ToInt32(openList[iX][iY][FATHER_DIR]))
                {
                    case DIR_DOWN: iY++; break;
                    case DIR_UP: iY--; break;
                    case DIR_LEFT: iX--; break;
                    case DIR_RIGHT: iX++; break;
                }
            }
            int size = route.Count();
            result = new Result[size + 1];
            for (int i = 0; i < size; i++)
            {
                result[i] = (Result)route.ElementAt(i);
            }
            result[size] = new Result(start_x, start_y);
            return result;
        }
    }
    public class Result
    {
        public Result(int x, int y)
        {
            this.x = x;
            this.y = y;
        }
        public int x
        {
            get;
            set;
        }
        public int y
        {
            get;
            set;
        }
    }
}

想减少路径中的拐点
131行到134行是本人加上去的,偶尔会有奇效,但在实测过程中并不是都能取到最少拐点的最短路径

解决方案:60分
本人对A*的研究不深,假如有说错还请LZ别喷。本人看的A*算法是八方向移动,也就是除了上下左右还有四个斜角可以走,能走斜角那应该不存在拐角多少问题,原因是走斜角一定比走拐角路径要短。本人粗看了一下,貌似LZ是四方向移动,所以才会遇到拐角问题,那么本人个人的思路是这样的,当有几步耗费是相同的情况下,应该优先选择和之前一步相同方向的步子。举个例子就是
   0 5 6                      4 5 6
   3 4                         3 4
1 2 3                      1 2 0
假设起点位1终点为6,0为障碍无法通过,那么当行进到2时,出现右方和上方的耗费相同,此时选上方则出现拐点,若沿着1->2的方向继续向右走则无拐点。同理走到3时也是如此。所以LZ看看能否可以向这个方向考虑下减少拐点

CodeBye 版权所有丨如未注明 , 均为原创丨本网站采用BY-NC-SA协议进行授权 , 转载请注明A*算法,在最短路径的基础上,本人还想减少拐点应该怎么做?
喜欢 (0)
[1034331897@qq.com]
分享 (0)