//método que arranca el algoritmo de pathfinding y establece los nodos inicial y final
private void findPath()
{
Node start = null;
Node end = null;
PathFinding path = new PathFinding(map);
start = createNode();
end = createNode();
path.init(start.X, start.Y, end.X, end.Y);
}
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);
}
public static List <Direction8Way> Search8WayNode(Node start, Node end, IMovable grid, out int distance)
{
Console.Error.WriteLine("Finding from {0} to {1} by A*", start, end);
distance = -1;
List <Direction8Way> pathWay = new List <Direction8Way>();
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 = 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)
{
Direction8Way dir = PathFinding.Get8WayDirection(parent.Parent.Pos, parent.Pos);
pathWay.Add(dir);
parent = parent.Parent;
}
found = true;
break;
}
List <SearchNode> neighbors = PathFinding.Get8WayNeighbors(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 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 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);
}
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);
}
