public AstarNode(int _x, int _y) //自定义构造函数 { this.x = _x; this.y = _y; this.parent = null; this.g = 0; this.h = 0; }
private void addToOpenList(AstarNode parent, int x, int y) //【方法】 将点加入开启列表(以parent为父节点在开启列表中加入点(x,y))【包含估价函数】 { int absX = Math.Abs(endX - x); int absY = Math.Abs(endY - y); if (!isValid(x, y)) //是否在地图中 { return; } if (inOpenList(x, y) || inCloseList(x, y)) //是否在列表中 { return; } if (!canWalk(x, y) && parent != null) //是否可走或有父节点 { return; } AstarNode node = new AstarNode(x, y); node.Parent = parent; if (parent == null) { node.G = 0; node.H = 0; } else { if (Math.Abs(parent.X - x) + Math.Abs(parent.Y - y) == 2) { node.G = 14; } else { node.G = 10; } node.H = 10 * ((Math.Abs(endX - x) + Math.Abs(endY - y)) + (1.414 - 2) * Math.Min(absX, absY)); //预估H:切比雪夫距离 } openList.Add(node); openList.Sort(delegate(AstarNode lhs, AstarNode rhs) //方法 按 lhs比rhs大的 升序排列 { if (lhs.F < rhs.F) { return(-1); } else if (lhs.F > rhs.F) { return(1); } return(0); }); }
private AstarNode openToCloseWithBest() //【方法】 将最优点由 开启列表 转到 关闭列表 { AstarNode node = getBestNodeFromOpenList(); if (node == null) { return(null); } openToClose(node); return(node); }
private AstarNode findNearPointFromList(List <AstarNode> list, int x, int y) //【方法】找列表中到点(x,y)之切比雪夫距离最小点 { AstarNode result = null; double minDistance = int.MaxValue; //MaxValue 一个特大值 foreach (AstarNode node in list) { //【下面距离公式用作开启列表节点筛选的引导】 double dist = node.F; if (dist < minDistance) { minDistance = dist; result = node; } } return(result); }
public List <AstarNode> findPath(int _startX, int _startY, int _endX, int _endY) //【方法】找路 { this.endX = _endX; this.endY = _endY; this.openList.Clear(); this.closeList.Clear(); List <AstarNode> result = new List <AstarNode>(); AstarNode currNode = null; bool findPathFlag = false; if (canWalk(endX, endY)) //添加。 防止因终点不可用造成崩溃 //终点可用 { addToOpenList(null, _startX, _startY); //此时Parent==null while (openList.Count > 0) { currNode = openToCloseWithBest(); //将最优点(开启列表第一个)由开启列表转到关闭列表 if (currNode == null) { break; } if (currNode.X == _endX && currNode.Y == _endY) { findPathFlag = true; break; } genAroundNode(currNode); } if (!findPathFlag) { currNode = findNearPointFromList(closeList, endX, endY); //此为欧几里得距离判断,减少不必要点的判断 } if (currNode == null) { return(null); } while (true) { result.Add(currNode); if (currNode.X == _startX && currNode.Y == _startY) { break; } currNode = currNode.Parent; } result.Reverse(); return(result); } else //终点不可用 { result = null; //停止循迹 return(result); } }
private void genAroundNode(AstarNode node) //【方法】 加入相邻点,八点以及直角障碍判别 { int x = node.X; int y = node.Y; addToOpenList(node, node.X - 1, node.Y); addToOpenList(node, node.X, node.Y - 1); addToOpenList(node, node.X, node.Y + 1); addToOpenList(node, node.X + 1, node.Y); if (!canWalk(x - 1, y) && canWalk(x, y + 1) && canWalk(x + 1, y) && canWalk(x, y - 1)) //1 { addToOpenList(node, node.X + 1, node.Y - 1); addToOpenList(node, node.X + 1, node.Y + 1); } else if (canWalk(x - 1, y) && !canWalk(x, y + 1) && canWalk(x + 1, y) && canWalk(x, y - 1)) //2 { addToOpenList(node, node.X - 1, node.Y - 1); addToOpenList(node, node.X + 1, node.Y - 1); } else if (canWalk(x - 1, y) && canWalk(x, y + 1) && !canWalk(x + 1, y) && canWalk(x, y - 1)) //3 { addToOpenList(node, node.X - 1, node.Y - 1); addToOpenList(node, node.X - 1, node.Y + 1); } else if (canWalk(x - 1, y) && canWalk(x, y + 1) && canWalk(x + 1, y) && !canWalk(x, y - 1)) //4 { addToOpenList(node, node.X - 1, node.Y + 1); addToOpenList(node, node.X + 1, node.Y + 1); } else if (!canWalk(x - 1, y) && !canWalk(x, y + 1) && canWalk(x + 1, y) && canWalk(x, y - 1)) //5 { addToOpenList(node, node.X + 1, node.Y - 1); } else if (canWalk(x - 1, y) && !canWalk(x, y + 1) && canWalk(x + 1, y) && !canWalk(x, y - 1)) //6 { } else if (canWalk(x - 1, y) && !canWalk(x, y + 1) && !canWalk(x + 1, y) && canWalk(x, y - 1)) //7 { addToOpenList(node, node.X - 1, node.Y - 1); } else if (!canWalk(x - 1, y) && canWalk(x, y + 1) && canWalk(x + 1, y) && !canWalk(x, y - 1)) //8 { addToOpenList(node, node.X + 1, node.Y + 1); } else if (!canWalk(x - 1, y) && canWalk(x, y + 1) && !canWalk(x + 1, y) && canWalk(x, y - 1)) //9 { } else if (canWalk(x - 1, y) && canWalk(x, y + 1) && !canWalk(x + 1, y) && !canWalk(x, y - 1)) //10 { addToOpenList(node, node.X - 1, node.Y + 1); } else if (!canWalk(x - 1, y) && canWalk(x, y + 1) && canWalk(x + 1, y) && canWalk(x, y - 1)) { } else if (!canWalk(x - 1, y) && canWalk(x, y + 1) && canWalk(x + 1, y) && canWalk(x, y - 1)) { } else if (!canWalk(x - 1, y) && canWalk(x, y + 1) && canWalk(x + 1, y) && canWalk(x, y - 1)) { } else if (!canWalk(x - 1, y) && canWalk(x, y + 1) && canWalk(x + 1, y) && canWalk(x, y - 1)) { } else if (!canWalk(x - 1, y) && canWalk(x, y + 1) && canWalk(x + 1, y) && canWalk(x, y - 1)) { } //15 全是障碍 else //16 没有障碍 { addToOpenList(node, node.X - 1, node.Y - 1); addToOpenList(node, node.X - 1, node.Y + 1); addToOpenList(node, node.X + 1, node.Y - 1); addToOpenList(node, node.X + 1, node.Y + 1); } //if (!canWalk(x - 1, y) && canWalk(x, y + 1) && canWalk(x + 1, y) && canWalk(x, y - 1)) { } }
private void openToClose(AstarNode node) //【方法】 开启列表 转到 关闭列表 { openList.Remove(node); closeList.Add(node); }