static List <HexSearchNode> GetHexNeighbors(HexSearchNode center, Offset endNode) { //Console.Error.WriteLine( "Finding neighbors of {0}", center ); List <HexSearchNode> neighbors = new List <HexSearchNode>(); var parity = center.Pos.row & 1; // 0 for even line, 1 for odd line var dir = Hexagonal.OffsetDirections[parity]; const int WIDTH = 23; const int HEIGHT = 21; // 0 : Right // 1 : Upper Right // 2 : Upper Left // 3 : Left // 4 : Lower left // 5 : Lower Right for (int i = 0; i < 6; ++i) { // not allowed 2,3,4 since they are only able to go ahead. if (i >= 2 && i <= 4) { continue; } // to sustain the direction of its previous direction int rotation = (6 + i + center.Rotation) % 6; Offset newPos = new Offset(center.Pos.row + dir[rotation, 1], center.Pos.col + dir[rotation, 0]); int direction = Offset.GetRotation(center.Pos, newPos); if (newPos.row < 0 || newPos.row >= HEIGHT) { continue; } if (newPos.col < 0 || newPos.col >= WIDTH) { continue; } // for specific constraints //if( IsMovable( newPos, direction ) == false ) // continue; int gridCost = 1; // + ( int )Caribbean.GetGridProperty( newPos ); HexSearchNode newNode = new HexSearchNode(newPos, rotation, center.CostSoFar + gridCost, PathFinding.GetHexHeuristic(newPos, endNode)); neighbors.Add(newNode); } return(neighbors); }
public static List <Offset> FindClosedPosByAStar(Offset startNode, int startDirection, Offset endNode, out List <int> rotationPathWay) { Console.Error.WriteLine("Path finding from {0} to {1} at direction of {2}", startNode, endNode, startDirection); rotationPathWay = new List <int>(); List <Offset> pathWay = new List <Offset>(); if (startNode.Equals(endNode)) { return(pathWay); } List <HexSearchNode> frontier = new List <HexSearchNode>(); List <HexSearchNode> explored = new List <HexSearchNode>(); HexSearchNode start = new HexSearchNode(startNode, startDirection, 0, GetHexHeuristic(startNode, endNode)); frontier.Add(start); bool found = false; while (frontier.Count > 0) { HexSearchNode current = frontier[0]; frontier.RemoveAt(0); explored.Add(current); if (current.Pos.Equals(endNode)) { HexSearchNode parent = current; while (parent != null && parent != start) { //Console.Error.WriteLine( "cost to {0} is {1}", parent.Pos, parent.CostSoFar ); int rotation = Offset.GetRotation(parent.Parent.Pos, parent.Pos); rotationPathWay.Add(rotation); pathWay.Add(parent.Pos); parent = parent.Parent; } found = true; break; } // constraints for early exit such as line of sight if (Offset.GetDistance(current.Pos, start.Pos) < 5) { List <HexSearchNode> neighbors = GetHexNeighbors(current, endNode); foreach (HexSearchNode node in neighbors) { if (explored.Contains(node)) { continue; } int costSoFar = current.CostSoFar; int costToEnd = PathFinding.GetHexHeuristic(node.Pos, endNode); int index = frontier.IndexOf(node); if (index > 0) { if (costSoFar < frontier[index].CostSoFar) { // already exist in the container and found better way frontier[index].Parent = current; frontier[index].Rotation = current.Rotation; frontier[index].CostSoFar = costSoFar; frontier[index].CostToEnd = costToEnd; } } else { // Not found node.Parent = current; node.CostSoFar = costSoFar; node.CostToEnd = costToEnd; frontier.Add(node); } } } frontier.Sort((item1, item2) => (item1.CostSoFar + item1.CostToEnd) - (item2.CostSoFar + item2.CostToEnd)); //Console.Error.WriteLine( "Frontier is {0}", frontier.ToDebugString() ); } if (pathWay.Count == 0 && explored.Count > 0) { explored.Sort((item1, item2) => (item1.CostSoFar + item1.CostToEnd) - (item2.CostSoFar + item2.CostToEnd)); HexSearchNode parent = explored[0]; while (parent != null && parent != start) { rotationPathWay.Add(Offset.GetRotation(parent.Parent.Pos, parent.Pos)); pathWay.Add(parent.Pos); parent = parent.Parent; } } pathWay.Reverse(); rotationPathWay.Reverse(); if (found) { Console.Error.WriteLine("Found : Pathway from {0} to target({1}) is {2}, rotation = {3} at distance of {4}", startNode, endNode, pathWay.ToDebugString(), rotationPathWay.ToDebugString(), pathWay.Count); } else { Console.Error.WriteLine("Not Found : Closest Pathway from {0} to target({1}) is {2}, rotation = {3} at distance of {4}", startNode, endNode, pathWay.ToDebugString(), rotationPathWay.ToDebugString(), pathWay.Count); } return(pathWay); }