public static List <Direction4Way> Search4WayNode(Node start, IReachable searchGrid, IMovable moveGrid, out int distance) { Console.Error.WriteLine("Finding from {0} by BFS", start); distance = -1; List <Direction4Way> pathWay = new List <Direction4Way>(); if (searchGrid.IsReachedDestination(start)) { distance = 0; return(pathWay); } List <SearchNode> frontier = new List <SearchNode>(); List <SearchNode> explored = new List <SearchNode>(); SearchNode startNode = new SearchNode(start, null, 0, 0); frontier.Add(startNode); bool found = false; while (frontier.Count > 0) { SearchNode current = ( SearchNode )frontier[0]; frontier.RemoveAt(0); explored.Add(current); if (searchGrid.IsReachedDestination(current.Pos)) { Console.Error.WriteLine("End node found : {0}", current.Pos); distance = current.CostSoFar + current.CostToEnd; SearchNode parent = current; while (parent != null && parent.Pos.Equals(start) == false) { Direction4Way dir = PathFinding.Get4WayDirection(parent.Parent.Pos, parent.Pos); //Console.Error.WriteLine( "iterate back to parent {0}", parent.Pos ); pathWay.Add(dir); parent = parent.Parent; } found = true; break; } List <SearchNode> neighbors = PathFinding.Get4WayNeighbors(current, moveGrid); foreach (SearchNode node in neighbors) { if (explored.Contains(node)) { continue; } node.CostSoFar = current.CostSoFar + 1; int index = frontier.IndexOf(node); if (index == -1) { frontier.Add(node); } } //Console.Error.WriteLine( "frontier = {0}", frontier.ToDebugString() ); } pathWay.Reverse(); if (found) { Console.Error.WriteLine("Found : from {0} through {1} of distance {2}", start, pathWay.ToDebugString(), distance); } else { Console.Error.WriteLine("No Way! Found from {0}", start); } return(pathWay); }
public static List <Direction4Way> Search4WayNode(Node start, Node end, IMovable grid, out int distance) { Console.Error.WriteLine("Finding from {0} to {1} by A*", start, end); distance = -1; List <Direction4Way> pathWay = new List <Direction4Way>(); if (start.Equals(end)) { distance = 0; return(pathWay); } List <SearchNode> frontier = new List <SearchNode>(); List <SearchNode> explored = new List <SearchNode>(); SearchNode startNode = new SearchNode(start, null, 0, PathFinding.GetManhattanHeuristic(start, end)); frontier.Add(startNode); bool found = false; while (frontier.Count > 0) { SearchNode current = ( SearchNode )frontier[0]; frontier.RemoveAt(0); explored.Add(current); if (current.Pos.Equals(end)) { distance = current.CostSoFar + current.CostToEnd; SearchNode parent = current; while (parent != null && parent.Pos.Equals(start) == false) { Direction4Way dir = PathFinding.Get4WayDirection(parent.Parent.Pos, parent.Pos); pathWay.Add(dir); parent = parent.Parent; } found = true; break; } List <SearchNode> neighbors = PathFinding.Get4WayNeighbors(current, grid); foreach (SearchNode node in neighbors) { if (explored.Contains(node)) { continue; } node.CostSoFar = current.CostSoFar + 1; node.CostToEnd = PathFinding.GetManhattanHeuristic(node.Pos, end); int index = frontier.IndexOf(node); if (index > 0) { if (node.CostSoFar < frontier[index].CostSoFar) { // if found better way frontier[index].Parent = current; frontier[index].CostSoFar = node.CostSoFar; frontier[index].CostToEnd = node.CostToEnd; } } else { frontier.Add(node); } } frontier.Sort((item1, item2) => (item1.CostSoFar + item1.CostToEnd) - (item2.CostSoFar + item2.CostToEnd)); //Console.Error.WriteLine( "frontier = {0}", frontier.ToDebugString() ); } pathWay.Reverse(); if (found) { Console.Error.WriteLine("Found : {0} to {1} : {2} of distance {3}", start, end, pathWay.ToDebugString(), distance); } else { Console.Error.WriteLine("No Way! : {0} to {1}", start, end); } return(pathWay); }
public List <Direction4Way> SearchGraphByDijkstra(Node start, PathGrid grid, out int cost) { cost = -1; List <Direction4Way> pathWay = new List <Direction4Way>(); if (grid.IsReachedDestination(start)) { cost = 0; return(pathWay); } List <SearchNode> frontier = new List <SearchNode>(); List <SearchNode> explored = new List <SearchNode>(); SearchNode startNode = new SearchNode(start, null, 0, 0); frontier.Add(startNode); bool found = false; while (frontier.Count > 0) { SearchNode current = ( SearchNode )frontier[0]; frontier.RemoveAt(0); explored.Add(current); if (grid.IsReachedDestination(current.Pos)) { cost = current.CostSoFar + current.CostToEnd; SearchNode parent = current; while (parent != null && parent.Pos.Equals(start) == false) { Direction4Way dir = PathFinding.Get4WayDirection(parent.Parent.Pos as Node, parent.Pos as Node); pathWay.Add(dir); parent = parent.Parent; } found = true; break; } List <SearchNode> neighbors = PathFinding.Get4WayNeighbors(current, grid); foreach (SearchNode node in neighbors) { if (explored.Contains(node)) { continue; } node.CostSoFar = current.CostSoFar + 1; node.CostToEnd = 0; int index = frontier.IndexOf(node); if (index > 0) { if (node.CostSoFar < frontier[index].CostSoFar) { // if found better way frontier[index].Parent = current; frontier[index].CostSoFar = node.CostSoFar; frontier[index].CostToEnd = node.CostToEnd; } } else { frontier.Add(node); } } frontier.Sort((item1, item2) => (item1.CostSoFar + item1.CostToEnd) - (item2.CostSoFar + item2.CostToEnd)); //Console.Error.WriteLine( "frontier = {0}", frontier.ToDebugString() ); } pathWay.Reverse(); if (found) { Console.Error.WriteLine("Found from {0} : {1} of distance {2}", start, pathWay.ToDebugString(), cost); } else { Console.Error.WriteLine("No Way! from {0}", start); } return(pathWay); }