public void InitMapInfo(int w, int h) { for (int i = 0; i < w; i++) { for (int j = 0; j < h; j++) { AStartNode node = new AStartNode(i, j, Random.Range(0, 100) < 20 ? NodeType.Stop : NodeType.Walk); } } }
public List <AStartNode> FindPath(Vector2 startPos, Vector2 endPos) { // 判断范围 省略 // 判断阻挡 省略 AStartNode start = nodes[(int)startPos.x, (int)startPos.y]; AStartNode end = nodes[(int)endPos.x, (int)endPos.y]; // 清空上次寻路数据 closeList.Clear(); openList.Clear(); start.father = null; start.f = 0; start.g = 0; start.h = 0; closeList.Add(start); while (true) { FindNearlyNodeToOpenList(start.x - 1, start.y - 1, 1.4f, start, end); FindNearlyNodeToOpenList(start.x, start.y - 1, 1f, start, end); FindNearlyNodeToOpenList(start.x + 1, start.y - 1, 1.4f, start, end); FindNearlyNodeToOpenList(start.x - 1, start.y, 1.4f, start, end); FindNearlyNodeToOpenList(start.x + 1, start.y, 1.4f, start, end); FindNearlyNodeToOpenList(start.x - 1, start.y + 1, 1.4f, start, end); FindNearlyNodeToOpenList(start.x, start.y + 1, 1f, start, end); FindNearlyNodeToOpenList(start.x + 1, start.y + 1, 1.4f, start, end); if (openList.Count == 0) { return(null); } openList.Sort(SortOpenList); closeList.Add(openList[0]); start = openList[0]; openList.RemoveAt(0); if (start == end) { List <AStartNode> path = new List <AStartNode>(); path.Add(end); while (end.father != null) { path.Add(end.father); end = end.father; } path.Reverse(); return(path); } } }
private int SortOpenList(AStartNode a, AStartNode b) { if (a.f >= b.f) { return(1); } else { return(-1); } }
//Find all nodes that surround a particiular node which are empty //it also takes into account you can only consider diagnol nodes empty if there is //no walls on either relevant side List <AStartNode> GetSurroundingEmptyNodes(AStartNode fromNode) { var emptyNodes = new List <AStartNode>(); var surroundingPoints = GetSurroundingPoints(new Vector2(fromNode.Point.x, fromNode.Point.y)); foreach (var surroundingPoint in surroundingPoints) { int x = (int)surroundingPoint.x; int y = (int)surroundingPoint.y; if (x < 0 || x >= m_width || y < 0 || y >= m_height) { continue; } var node = m_aStartWorld[x, y]; if (!node.IsEmpty) { continue; } if (node.State == AStarState.Closed) { continue; } //Only to be added if their G-value is lower. This means that this route is lower for this node than the previously explored way that considered it if (node.State == AStarState.Open) { var preG = fromNode.G + node.Cost; if (preG < node.G) { node.Load(fromNode); emptyNodes.Add(node); } continue; } else { //If untested, set the parent and flat it as Open for consideration node.Load(fromNode); node.State = AStarState.Open; emptyNodes.Add(node); } } return(emptyNodes); }
//The method that encases all the logic of A*'s actual path finding. //Returns true if a path can be found, false it not. //The root parameter is the starting point of the search bool Search(AStartNode root) { //All the branches hit of possible paths as the best path is found. //The branches are organized by combined F values var nodes = new SortedDictionary <float, Queue <AStartNode> >(); //A action to add a node to the relevant path Action <AStartNode> AddNodeToNodes = (node) => { if (!nodes.ContainsKey(node.F)) { nodes.Add(node.F, new Queue <AStartNode>()); } nodes[node.F].Enqueue(node); }; //A action that finds the node with the lowset F and dequeues it from the list of nodes to try Func <AStartNode> GetLowestFs = () => { foreach (var key in nodes.Keys.ToList()) { if (nodes[key].Count > 0) { return(nodes[key].Dequeue()); } else { nodes.Remove(key); } } return(null); }; AddNodeToNodes(root); AStartNode currentNode = GetLowestFs(); //The 'recusive' loop, written as a do:while do { //Mark the current node to check as closed currentNode.State = AStarState.Closed; var surroundingEmptyNodes = GetSurroundingEmptyNodes(currentNode).OrderBy(x => x.F).ToList(); bool finish = false; //For every surrounding empty node ordered by F values foreach (var surroundingNode in surroundingEmptyNodes) { //If we found the end, mark it as finished and leave the loop if (surroundingNode.Point == m_endPoint) { finish = true; surroundingNode.Load(currentNode); break; //Otherwise, add the current node to the Nodes dictionary } else { surroundingNode.Load(currentNode); AddNodeToNodes(surroundingNode); } } //If we are finished, break out of the loop as we found the best path if (finish) { break; } //Set the current node to check to be that with the lowest F value in the nodes we can check next currentNode = GetLowestFs(); } while (currentNode != null); //Return success or fail return(currentNode != null); }