//计算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); }