A star算法
刚接触Silverlight没多久,花了点时间写了一个A星算法,大概花了一周的时间。寻迹速度在我T2400的电脑上,对角打开平均30秒左右。
不开对角是16秒.
刚开始学习,不懂代码的优化。贴上来请高手指教。
代码
using System;
using System.Net;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Documents;
using System.Windows.Ink;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Animation;
using System.Windows.Shapes;
using System.Collections.Generic;
namespace AStar.Algorithm
{
#region 节点
public class PathNode
{
public int X;
public int Y;
public int G;
public int H;
public int F;
public PathNode parentPathNode;
}
#endregion
public class FindPath
{
#region 变量定义
List<PathNode> openNodeList = new List<PathNode>();
List<PathNode> closeNodeList = new List<PathNode>();
private byte[,] matrix;
private Point startPoint;
private Point endPoint;
private sbyte[,] direction;
private bool diagonal;
private bool found = false;
PathNode newPathNode;
private PathNode minPathNode;
#endregion
public FindPath(byte[,] _matrix) //构造方法
{
this.matrix = _matrix;
}
public List<Point> StartFindPath(Point _startPoint, Point _endPoint, bool diagonal) //查找路径
{
openNodeList.Clear();
closeNodeList.Clear();
this.startPoint = _startPoint;
this.endPoint = _endPoint;
this.diagonal = diagonal;
PathNode startPathNode = new PathNode();
startPathNode.X =(int)startPoint.X;
startPathNode.Y = (int)startPoint.Y;
startPathNode.G = 0;
startPathNode.F = 0;
startPathNode.H = 0;
direction= diagonal? new sbyte[,]{{0,1},{1,1},{1,0},{1,-1},{0,-1},{-1,-1},{-1,0},{-1,1}}:
new sbyte[,]{{0,1},{1,0},{0,-1},{-1,0}};
CheakAround(startPathNode);
minPathNode = findMix(openNodeList);
while (!found)
{
if (minPathNode == null )
{
MessageBox.Show("找不到路径", "提示", MessageBoxButton.OK);
found = true;
continue;
}
if (minPathNode.X != endPoint.X || minPathNode.Y != endPoint.Y)
{
CheakAround(minPathNode);
minPathNode = findMix(openNodeList);
}
else
{
found = true;
}
}
if (openNodeList.Count!=0)
{
return GetPathPoinList(minPathNode , null);
}
else
{
return null;
}
}
public void CheakAround(PathNode _pathNode) //检查节点周围的节点;
{
PathNode inPathNode = _pathNode;
newPathNode = new PathNode();
bool isClose = false;
bool isOpen = false;
openNodeList.Remove(inPathNode);
closeNodeList.Add(inPathNode);
for (int i = 0; i < (diagonal? 8 : 4); i++)
{
newPathNode = new PathNode();
newPathNode.X = inPathNode.X + direction[i, 0];
newPathNode.Y = inPathNode.Y + direction[i, 1];
newPathNode.G = inPathNode.G + ((direction[i, 0] + direction[i, 1]) == 1 || (direction[i, 0] + direction[i, 1] == -1) ? 10 : 14);
newPathNode.H = (Math.Abs(newPathNode.X - (int)endPoint.X )) + (Math.Abs(newPathNode.Y - (int)endPoint.Y));
newPathNode.F = newPathNode.G + newPathNode.H;
newPathNode.parentPathNode = inPathNode;
foreach (PathNode pNode in closeNodeList)
{
if (pNode.X == newPathNode.X && pNode.Y == newPathNode.Y)
{
isClose = true;
break;
}
}
if (newPathNode.X < 0 || newPathNode.Y < 0 || newPathNode.X > matrix.GetUpperBound(0) || newPathNode.Y > matrix.GetUpperBound(1) || isClose) //节点超出范围返回继续检查;
{
isClose = false;
continue;
}
if (matrix[newPathNode.X, newPathNode.Y] == 1) //当前点是障碍物则返回继续检查;
{
continue;
}
PathNode isOpenNode =null ;
foreach (PathNode pNode in openNodeList)
{
if (pNode.X == newPathNode.X && pNode.Y == newPathNode.Y)
{
isOpen = true;
isOpenNode = pNode;
continue;
}
}
if (isOpen)
{
if (newPathNode.G >= isOpenNode.G)
{
//openNodeList.Remove(newPathNode);
isOpen = false;
continue;
}
else
{
openNodeList.Remove(inPathNode);
inPathNode.G = newPathNode.G;
inPathNode.H = newPathNode.H;
inPathNode.F = newPathNode.F;
openNodeList.Add(inPathNode);
isOpen = false;
continue;
}
}
openNodeList.Add(newPathNode);
}
}
private List<Point> GetPathPoinList(PathNode p,List<Point> ListPoint)
{
if (ListPoint == null)
{
ListPoint = new List<Point>();
}
if (p.parentPathNode != null)
{
Point point = new Point(p.parentPathNode.X, p.parentPathNode.Y);
ListPoint.Add(point);
GetPathPoinList(p.parentPathNode, ListPoint);
}
return ListPoint;
}
private PathNode findMix(List<PathNode> pathlist)
{
PathNode minNode=null;
if (pathlist.Count != 0)
{
minNode = pathlist[0];
foreach (PathNode p in pathlist)
{
if (minNode.F > p.F)
{
minNode = p;
}
}
return minNode;
}
return minNode;
}
}
}