public bool init(IAStarGraph graph) { if (graph == null) { return(false); } mGraph = graph; mFValues = new float[mGraph.getCellCount()]; mGValues = new float[mGraph.getCellCount()]; mOpenTable = new uint[mGraph.getCellCount()]; mCloseTable = new uint[mGraph.getCellCount()]; mShortestPathTree = new uint[mGraph.getCellCount()]; for (int i = 0; i < mGraph.getCellCount(); i++) { mFValues[i] = 0.0f; mGValues[i] = 0.0f; mOpenTable[i] = 0; mCloseTable[i] = 0; mShortestPathTree[i] = 0; } mPriorityQueue.Init(mFValues); mSessionId = 0; return(true); }
/// <summary> /// 获取一条路径 /// </summary> /// <param name="graph">节点关系图</param> /// <param name="from">起点</param> /// <param name="to">终点</param> /// <returns>如果没有找到路径返回NULL</returns> public static List <AStarNode> FindPath(IAStarGraph graph, AStarNode from, AStarNode to) { // 清空上次寻路数据 graph.ClearTemper(); _openList.Clear(); _closedList.Clear(); // 开始寻找路径 _openList.Add(from); while (_openList.Count > 0) { // 获取当前代价值最小的节点 AStarNode current = _openList[0]; for (int i = 1; i < _openList.Count; i++) { if (_openList[i].Cost < current.Cost) { current = _openList[i]; } } _openList.Remove(current); _closedList.Add(current); // 成功找到终点 if (current == to) { return(RetracePath(from, to)); } // 获取邻居节点并添加到开放列表 foreach (AStarNode neighbor in graph.Neighbors(current)) { if (neighbor == null || neighbor.IsBlock() || _closedList.Contains(neighbor)) { continue; } float newCostToNeighbor = current.G + graph.CalculateCost(current, neighbor); if (newCostToNeighbor < neighbor.G || _openList.Contains(neighbor) == false) { neighbor.G = newCostToNeighbor; neighbor.H = graph.CalculateCost(neighbor, to); neighbor.Parent = current; if (_openList.Contains(neighbor) == false) { _openList.Add(neighbor); } } } } // 没有找到路径 return(null); }