/// <summary> /// 欧几里得估价函数 /// h(n) = D * sqrt((n.x - goal.x)^2 + (n.y - goal.y)^2) /// </summary> /// <param name="node">起始节点</param> /// <returns>欧几里得距离</returns> private double Euclidian(AStarGridNode node) { double distance_x = node.Position.x - Grid.EndNode.Position.x; double distance_y = node.Position.y - Grid.EndNode.Position.y; return(Math.Sqrt((distance_x * distance_x) + (distance_y * distance_y))); }
private bool IsOpen(AStarGridNode node) { IEnumerator enumerator = mOpenTable.GetEnumerator(); try { while (enumerator.MoveNext()) { AStarGridNode current = (AStarGridNode)enumerator.Current; if (current.Equals(node)) { return(true); } } } finally { IDisposable disposable = enumerator as IDisposable; if (disposable == null) { } disposable.Dispose(); } return(false); }
// Methods private void OnDrawGizmos() { if (astar != null) { AStarGrid grid = astar.Grid; for (int i = 0; i < grid.NumCols; i++) { for (int j = 0; j < grid.NumRows; j++) { AStarGridNode nodeAtPosition = grid.GetNodeAtPosition(i, j); if (drawNodesCenter) { Gizmos.color = Color.cyan; Gizmos.DrawWireSphere((Vector3)nodeAtPosition.Position, centerSize); } } } ArrayList path = astar.Path; if (path != null) { for (int k = 0; k < path.Count; k++) { AStarGridNode node2 = path[k] as AStarGridNode; if ((node2 != null) && drawPathNodesCenter) { Gizmos.color = Color.blue; Gizmos.DrawWireSphere((Vector3)node2.Position, centerSize); } } } } }
/// <summary> /// 构建路径信息 /// </summary> private void BuildPath() { Path.Clear(); for (AStarGridNode node = Grid.EndNode; false == node.Equals(Grid.StartNode); node = node.ParentNode) { Path.Add(node); } }
/// <summary> /// 对角线估价函数 /// h(n) = (D2 * h_diagonal(n)) + D * (h_straight(n) - 2 * h_diagonal(n))) /// h_diagonal(n) = min(abs(n.x - goal.x), abs(n.y - goal.y)) /// h_straight(n) = (abs(n.x - goal.x) + abs(n.y - goal.y)) /// </summary> /// <param name="node">起始节点</param> /// <returns>对角线距离</returns> private double Diagonal(AStarGridNode node) { double distance_x = Math.Abs((float)(node.Position.x - Grid.EndNode.Position.x)); double distance_y = Math.Abs((float)(node.Position.y - Grid.EndNode.Position.y)); double h_diagonal = Math.Min(distance_x, distance_y); double h_straight = distance_x + distance_y; return((mDiagonalCost * h_diagonal) + (mStraightCost * (h_straight - (2.0 * h_diagonal)))); }
/// <summary> /// 估价函数H的计算方法 /// </summary> /// <param name="node">A*节点</param> /// <returns>代价值</returns> private double GetHeruistic(AStarGridNode node) { switch (mHeuristicType) { case Heuristic.DIAGONAL: return(Diagonal(node)); case Heuristic.MANHATTAN: return(Manhattan(node)); case Heuristic.EUCLIDIAN: return(Euclidian(node)); } // 默认为对角线 return(Diagonal(node)); }
public void Init(int cols, int rows) { mNodes = new ArrayList(); NumCols = cols; NumRows = rows; for (int i = 0; i < NumCols; i++) { ArrayList list = new ArrayList(); for (int j = 0; j < NumRows; j++) { AStarGridNode node = new AStarGridNode(i, j, true); list.Add(node); } mNodes.Add(list); } }
private bool Search() { AStarGridNode startNode = Grid.StartNode; // 还没有遍历到最后一个节点 while (false == startNode.Equals(Grid.EndNode)) { int num = (int)Math.Max((float)0f, (float)(startNode.Position.x - 1f)); int num2 = (int)Math.Min((float)(Grid.NumCols - 1), startNode.Position.x + 1f); int num3 = (int)Math.Max((float)0f, (float)(startNode.Position.y - 1f)); int num4 = (int)Math.Min((float)(Grid.NumRows - 1), startNode.Position.y + 1f); for (int i = num; i <= num2; i++) { for (int j = num3; j <= num4; j++) { AStarGridNode nodeAtPosition = Grid.GetNodeAtPosition(i, j); // 剔除重复的节点 if (false == nodeAtPosition.Equals(startNode) && nodeAtPosition.Walkable) { double straightCost = mStraightCost; if ((startNode.Position.x != nodeAtPosition.Position.x) && (startNode.Position.y != nodeAtPosition.Position.y)) { straightCost = mDiagonalCost; } double num8 = startNode.G + (straightCost * nodeAtPosition.Cost); double heruistic = GetHeruistic(nodeAtPosition); double num10 = num8 + heruistic; // 已经在open或者close表中 if (IsOpen(nodeAtPosition) || IsClosed(nodeAtPosition)) { if (nodeAtPosition.F > num10) { nodeAtPosition.F = num10; nodeAtPosition.G = num8; nodeAtPosition.H = heruistic; nodeAtPosition.ParentNode = startNode; } } // 不在这些表中 else { nodeAtPosition.F = num10; nodeAtPosition.G = num8; nodeAtPosition.H = heruistic; nodeAtPosition.ParentNode = startNode; mOpenTable.Add(nodeAtPosition); } } } } // 已经走过该节点 mClosedTable.Add(startNode); // 检查是否没有需要走的节点了 if (mOpenTable.Count == 0) { return(false); } // 排序open表提高效率 mOpenTable.Sort(new AStarComparer()); // 获取第一个节点然后从open表中删除 startNode = (AStarGridNode)mOpenTable[0]; mOpenTable.RemoveAt(0); } BuildPath(); return(true); }
/// <summary> /// 曼哈顿估价函数 /// H(n) = D * (abs(n.x – goal.x) + abs(n.y – goal.y)) /// </summary> /// <param name="node">起始节点</param> /// <returns>曼哈顿距离</returns> private double Manhattan(AStarGridNode node) { return(mStraightCost * (Math.Abs((float)(node.Position.x - Grid.EndNode.Position.x)) + Math.Abs((float)(node.Position.y - Grid.EndNode.Position.y)))); }