//计算H值
public int CalcDist(Pnt start, Pnt goal)
{
int distX = (int)(Math.Abs(start.X - goal.X) * 10);
int distY = (int)(Math.Abs(start.Y - goal.Y) * 10);
return(distX + distY);
//return (10*(distX+distY)+(14-20)*Math.Min(distX,distY));
}
public int IsExist(Pnt item)
{
for (int i = 0; i < this.curPos; i++)
{
if (item.X == this.items[i].X && item.Y == this.items[i].Y)
{
return(i);
}
}
return(-1);
}
public void InsertItem(Pnt item)
{
int i = 0;
if (this.curPos == this.size - 1)
{
throw new Exception("The heap is fulfilled, can't add item anymore.");
}
for (i = ++(this.curPos); this.items[i / 2].F > item.F; i = i / 2)
{
this.items[i] = this.items[i / 2];
}
this.items[i] = item;
}
public bool FindPath(Point start, Point end, Graphics gra, Pen myPen)
{
Pnt pnt_start = new Pnt();
Pnt pnt_end = new Pnt();
pnt_start.X = start.X;
pnt_start.Y = start.Y;
pnt_end.X = end.X;
pnt_end.Y = end.Y;
OpenList.InsertItem(pnt_start);
while (OpenList.CurPos != 0)
{
//找出F值最小的点
var tempStart = OpenList.PopMinItem();
//myPen.Color = Color.Red;
//gra.DrawEllipse(myPen, tempStart.X, tempStart.Y, 1, 1);
//将F值最小点添加到闭集合
CloseList[tempStart.X, tempStart.Y] = 1;
int row, col;
for (int i = 0; i < 8; i++)
{
//列 为横坐标 对应width
row = tempStart.X + twds[i].x;
//行 为纵坐标 对应height
col = tempStart.Y + twds[i].y;
//在地图内部
if (row >= 0 && row < width && col >= 0 && col < height)
{
//不是障碍物且不在closed列表里
if (CloseList[row, col] != 1 && MazeArray[row, col] != 1)
{
if (row != end.X || col != end.Y)
{
Pnt tmpNode = new Pnt();
tmpNode.X = row;
tmpNode.Y = col;
tmpNode.H = CalcDist(tmpNode, pnt_end);
tmpNode.G = tempStart.G + twds[i].cost;// +costMap[col * map.width + row];
tmpNode.F = tmpNode.H + tmpNode.G;
//如果节点已经在open列表中，比较之前的fvalue和新的fvalue
int index = OpenList.IsExist(tmpNode);
//如果已经存在
if (index != -1)
{
if (OpenList.items[index].F > tmpNode.F)
{
OpenList.items[index].F = tmpNode.F;
OpenList.items[index].G = tmpNode.G;
//改变tmpNode的父节点索引
parents[row, col].X = tempStart.X;
parents[row, col].Y = tempStart.Y;
}
}
else
{
//添加tmpNode的父节点索引
parents[row, col].X = tempStart.X;
parents[row, col].Y = tempStart.Y;
OpenList.InsertItem(tmpNode);
}
}
else
{
//添加tmpNode的父节点索引
parents[row, col].X = tempStart.X;
parents[row, col].Y = tempStart.Y;
//SmoothPath();
return(true);
}
}
}
}
}
return(false);
}
