public static BreadCrumb FindPath(Grid world, Point start, Point end) { var bc = FindPathReversed(world, start, end); var temp = new BreadCrumb[256]; if (bc != null) { var index = 0; while (bc != null) { temp[index] = bc; bc = bc.Next; index++; } index -= 2; var current = new BreadCrumb(start); var head = current; while (index >= 0) { current.Next = new BreadCrumb(temp[index].Position); current = current.Next; index--; } return(head); } else { return(null); } }
/// <summary> /// Get node list in world position. /// </summary> /// <param name="node">Node to tranverse through.</param> /// <returns>The list of nodes in world position.</returns> private List <Vector3> GetNodeList(BreadCrumb node) { var nodeList = new List <Vector3>(); while (node != null) { nodeList.Add(GridToWorld(node.Position)); node = node.Next; } return(nodeList); }
private static BreadCrumb FindPathReversed(Grid world, Point start, Point end) { var openList = new MinHeap <BreadCrumb>(256); var brWorld = new BreadCrumb[world.Right, world.Top]; var current = new BreadCrumb(start) { Cost = 0 }; var finish = new BreadCrumb(end); brWorld[current.Position.X, current.Position.Y] = current; openList.Add(current); while (openList.Count > 0) { //Find best item and switch it to the 'closedList' current = openList.ExtractFirst(); current.OnClosedList = true; //Find neighbours for (var i = 0; i < Surrounding.Length; i++) { var tmp = new Point(current.Position.X + Surrounding[i].X, current.Position.Y + Surrounding[i].Y); if (!world.ConnectionIsValid(current.Position, tmp)) { continue; } try { //Check if we've already examined a neighbour, if not create a new node for it. BreadCrumb node; if (brWorld[tmp.X, tmp.Y] == null) { node = new BreadCrumb(tmp); brWorld[tmp.X, tmp.Y] = node; } else { node = brWorld[tmp.X, tmp.Y]; } //If the node is not on the 'closedList' check it's new score, keep the best if (!node.OnClosedList) { var diff = 0; if (current.Position.X != node.Position.X) { diff += 1; } if (current.Position.Y != node.Position.Y) { diff += 1; } var distance = (int)Mathf.Pow( Mathf.Max(Mathf.Abs(end.X - node.Position.X), Mathf.Abs(end.Y - node.Position.Y)), 2); var cost = current.Cost + diff + distance; if (cost < node.Cost) { node.Cost = cost; node.Next = current; } //If the node wasn't on the openList yet, add it if (!node.OnOpenList) { //Check to see if we're done if (node.Equals(finish)) { node.Next = current; return(node); } node.OnOpenList = true; openList.Add(node); } } } catch (Exception e) { Debug.LogWarning(tmp); } } } return(null); //no path found }