public static AStarPath GetPath(Vector3 vStartPosition, Vector3 vEndPosition) { AStarPath path = new AStarPath(); AStarNode pStartNode = GetClosestNode(vStartPosition); AStarNode pEndNode = GetClosestNode(vEndPosition); Vector3 vEndNodePosition = pEndNode.transform.position; Dictionary <AStarNode, AStarNodeInfo> nodeInfos = new Dictionary <AStarNode, AStarNodeInfo>(); List <AStarNodeInfo> openList = new List <AStarNodeInfo>(); List <AStarNode> closedList = new List <AStarNode>(); // Add start node to open list AddNodeToOpenList(pStartNode, null, nodeInfos, openList, closedList, vEndNodePosition); AStarNodeInfo pFinalNode = null; while (openList.Count > 0) { // Examine node AStarNodeInfo pCurrentNode = openList[0]; openList.RemoveAt(0); if (pCurrentNode.mNode == pEndNode) { pFinalNode = pCurrentNode; break; } else { foreach (AStarNode pNode in pCurrentNode.mNode.Links) { AddNodeToOpenList(pNode, pCurrentNode, nodeInfos, openList, closedList, vEndNodePosition); } } } // Retrace path if (pFinalNode != null) { AStarNodeInfo pNode = pFinalNode; while (pNode != null) { path.InsertNode(pNode.mNode); pNode = pNode.mPreviousNode; } } return(path); }
static void AddNodeToOpenList(AStarNode pNode, AStarNodeInfo parent, Dictionary <AStarNode, AStarNodeInfo> nodeInfos, List <AStarNodeInfo> openList, List <AStarNode> closedList, Vector3 vEndPosition) { AStarNodeInfo info = null; if (!nodeInfos.TryGetValue(pNode, out info)) { info = new AStarNodeInfo(); info.mNode = pNode; info.mCostH = Vector3.Distance(pNode.transform.position, vEndPosition); } float fGCost = 0.0f; if (parent != null) { fGCost = parent.mCostG + Vector3.Distance(pNode.transform.position, parent.mNode.transform.position); } if (fGCost < info.mCostG) { info.mPreviousNode = parent; info.mCostG = fGCost; info.mCostF = info.mCostG + info.mCostH; int iIndex = openList.Count; for (int i = 0; i < iIndex; ++i) { if (info.mCostF < openList[i].mCostF) { iIndex = i; break; } } openList.Remove(info); openList.Insert(iIndex, info); } }
//===================== map base function ================================ //Astar public List <MapCoordinates> FindPath(MapCoordinates start, MapCoordinates end) { List <AStarNodeInfo> openList = new List <AStarNodeInfo>(); bool[,] closeArray = new bool[mapGrid.width, mapGrid.height]; var keyNode = new AStarNodeInfo(); keyNode.pos = start; keyNode.gWeight = 0; keyNode.hWeight = MapCoordinates.Distance(start, end); openList.Add(keyNode); Vector2Int[] neighbourOffsets = new Vector2Int[4] { new Vector2Int(-1, 0), new Vector2Int(1, 0), new Vector2Int(0, 1), new Vector2Int(0, -1) }; while (openList.Count > 0) { AStarNodeInfo curNode = openList[0]; openList.RemoveAt(0); //check if curNode is end if (curNode.pos.Equal(end)) { keyNode = curNode; break; } //insert neighbours for (int i = 0; i < neighbourOffsets.Length; ++i) { MapCoordinates neighbourPos = new MapCoordinates(curNode.pos.X + neighbourOffsets[i].x, curNode.pos.Z + neighbourOffsets[i].y); if (!IsMapCoordCanMove(neighbourPos) || closeArray[neighbourPos.X, neighbourPos.Z]) { continue; } //check if openList has contained same node AStarNodeInfo sameNode = openList.Find((AStarNodeInfo node) => { return(node.pos.Equal(neighbourPos)); }); if (sameNode != null) { if (sameNode.gWeight > curNode.gWeight + 1) { sameNode.parent = curNode; sameNode.gWeight = curNode.gWeight + 1; } continue; } AStarNodeInfo newNode = new AStarNodeInfo(); newNode.pos = neighbourPos; newNode.gWeight = curNode.gWeight + 1; newNode.hWeight = MapCoordinates.Distance(neighbourPos, end); newNode.parent = curNode; openList.Add(newNode); } closeArray[curNode.pos.X, curNode.pos.Z] = true; openList.Sort((AStarNodeInfo a, AStarNodeInfo b) => { return((a.gWeight + a.hWeight) - (b.gWeight + b.hWeight)); }); } //return astar result-path List <MapCoordinates> path = new List <MapCoordinates>(); while (keyNode != null) { path.Insert(0, keyNode.pos); keyNode = keyNode.parent; } return(path); }