public static List <Position> FindPath(IEntity entity, Room room, Position start, Position end)
{
List <Position> Path = new List <Position>();
PathfinderNode Nodes = FindPathReversed(entity, room, end, start);
if (Nodes != null) // make sure we do have a path first
{
while (Nodes.Next != null)
{
Path.Add(Nodes.Next.Position);
Nodes = Nodes.Next;
}
}
// I need to change 'IsValidStep' to not count the position the user is on (the user who wants to walk) or the emulator will error..
return(Path);
}
private static PathfinderNode FindPathReversed(IEntity entity, Room room, Position start, Position end)
{
MinHeap <PathfinderNode> openList = new MinHeap <PathfinderNode>(256);
PathfinderNode[,] map = new PathfinderNode[room.Model.MapSizeX, room.Model.MapSizeY];
PathfinderNode node;
Position tmp;
int Cost;
int Diff;
PathfinderNode current = new PathfinderNode(start);
current.Cost = 0;
PathfinderNode Finish = new PathfinderNode(end);
map[current.Position.X, current.Position.Y] = current;
openList.Add(current);
while (openList.Count > 0)
{
current = openList.ExtractFirst();
current.InClosed = true;
for (int i = 0; NoDiag?i < NoDiagMovePoints.Length : i < MovePoints.Length; i++)
{
tmp = current.Position + (NoDiag ? NoDiagMovePoints[i] : MovePoints[i]);
bool isFinalMove = (tmp.X == end.X && tmp.Y == end.Y); // are we at the final position?
if (IsValidStep(room, entity, new Position(current.Position.X, current.Position.Y), tmp, isFinalMove)) // need to set the from positions
{
if (map[tmp.X, tmp.Y] == null)
{
node = new PathfinderNode(tmp);
map[tmp.X, tmp.Y] = node;
}
else
{
node = map[tmp.X, tmp.Y];
}
if (!node.InClosed)
{
Diff = 0;
if (current.Position.X != node.Position.X)
{
Diff += 2;
}
if (current.Position.Y != node.Position.Y)
{
Diff += 2;
}
Cost = current.Cost + Diff + node.Position.GetDistanceSquared(end);
if (Cost < node.Cost)
{
node.Cost = Cost;
node.Next = current;
}
if (!node.InOpen)
{
if (node.Equals(Finish))
{
node.Next = current;
return(node);
}
node.InOpen = true;
openList.Add(node);
}
}
}
}
}
return(null);
}