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行是我加上去的，偶尔会有奇效，但在实测过程中并不是都能取到最少拐点的最短路径
------解决思路----------------------
我对A*的研究不深，如果有说错还请楼主别喷。我看的A*算法是八方向移动，也就是除了上下左右还有四个斜角可以走，能走斜角那应该不存在拐角多少问题，因为走斜角一定比走拐角路径要短。我粗看了一下，貌似楼主是四方向移动，所以才会遇到拐角问题，那么我个人的思路是这样的，当有几步耗费是相同的情况下，应该优先选择和之前一步相同方向的步子。举个例子就是
0 5 6 4 5 6
3 4 3 4
1 2 3 1 2 0
假设起点位1终点为6，0为障碍无法通过，那么当行进到2时，出现右方和上方的耗费相同，此时选上方则出现拐点，若沿着1->2的方向继续向右走则无拐点。同理走到3时也是如此。所以楼主看看是否可以向这个方向考虑下减少拐点