public SysClG.List <LibPF.Position> FindPath(LibPF.Grid grid, LibPF.Position start, LibPF.Position end, LibPF.Offset[] movementPattern, int iterationLimit)
{
#if DEBUG
this.ClearStepList();
#endif
if (start == end)
{
return(new SysClG.List <LibPF.Position> {
start
});
}
LibPF.Finder.MinHeapNode head = new LibPF.Finder.MinHeapNode(start, this.ManhattanDistance(start, end));
LibPF.Finder.MinHeap open = new LibPF.Finder.MinHeap();
open.Push(head);
float[] costSoFar = new float[grid.DimX * grid.DimY];
LibPF.Position[] cameFrom = new LibPF.Position[grid.DimX * grid.DimY];
while (open.HasNext() && iterationLimit > 0)
{
LibPF.Position current = open.Pop().Position;
#if DEBUG
this.MessageCurrent(current, this.PartiallyReconstructPath(grid, start, current, cameFrom));
#endif
if (current == end)
{
return(this.ReconstructPath(grid, start, end, cameFrom));
}
this.StepOn(grid, open, cameFrom, costSoFar, movementPattern, current, end);
#if DEBUG
this.MessageClose(current);
#endif
--iterationLimit;
}
return(null);
}
private void StepOn(LibPF.Grid grid, LibPF.Finder.MinHeap open, LibPF.Position[] cameFrom, float[] costSoFar, LibPF.Offset[] movementPattern, LibPF.Position current, LibPF.Position end)
{
float initialCost = costSoFar[grid.GetIndexUnchecked(current.X, current.Y)];
foreach (LibPF.Offset option in this.GetMovementOptions(current, grid.DimX, grid.DimY, movementPattern))
{
LibPF.Position position = current + option;
float cellCost = grid.GetCellCostUnchecked(position);
if (float.IsInfinity(cellCost))
{
continue;
}
int index = grid.GetIndexUnchecked(position.X, position.Y);
float newCost = initialCost + cellCost * option.Cost;
float oldCost = costSoFar[index];
if (!(oldCost <= 0) && !(newCost < oldCost))
{
continue;
}
costSoFar[index] = newCost;
cameFrom[index] = current;
newCost = newCost + this.ManhattanDistance(position, end);
open.Push(new LibPF.Finder.MinHeapNode(position, newCost));
#if DEBUG
this.MessageOpen(position);
#endif
}
}
private SysClG.List <LibPF.Position> ReconstructPath(LibPF.Grid grid, LibPF.Position start, LibPF.Position end, LibPF.Position[] cameFrom)
{
SysClG.List <LibPF.Position> path = new SysClG.List <LibPF.Position> {
end
};
LibPF.Position current = end;
do
{
LibPF.Position previous = cameFrom[grid.GetIndexUnchecked(current.X, current.Y)];
current = previous;
path.Add(current);
} while (current != start);
return(path);
}
public static void Run(int SizeX, int SizeY, LibPF.Position start, LibPF.Position end, params LibPF.Position[] blocks)
{
if (SizeX > 99)
{
SizeX = 99;
}
if (SizeY > 99)
{
SizeY = 99;
}
LibPF.Grid g = new LibPF.Grid(SizeX, SizeY, defaultCost: 1.0f);
foreach (LibPF.Position block in blocks)
{
g.BlockCell(block);
}
long preTime = Sys.DateTime.Now.Ticks;
string AUX = g.ToJSon();
g.Reset(1, 1, defaultCost: 1.0f);
g.FromJSon(AUX);
if (g.ToJSon() == AUX)
{
long posTime = Sys.DateTime.Now.Ticks;
AUX = string.Empty;
Sys.Console.Write("Time (JSon Conversion): " + Sys.TimeSpan.FromTicks(posTime - preTime).ToString());
Sys.Console.WriteLine();
preTime = Sys.DateTime.Now.Ticks;
LibPF.Position[] path = g.GetPath(start, end, LibPF.MovementPatterns.Full);
posTime = Sys.DateTime.Now.Ticks;
Sys.Console.WriteLine("Time (Find Path): " + Sys.TimeSpan.FromTicks(posTime - preTime).ToString());
preTime = Sys.DateTime.Now.Ticks;
Sys.Console.WriteLine();
Sys.Console.WriteLine();
Sys.Console.Write(" _");
for (int x = 0; x < SizeX; x++)
{
Sys.Console.Write("_" + ((x < 10) ? ("_" + x.ToString()) : x.ToString()) + "___");
}
Sys.Console.WriteLine();
Sys.ConsoleColor stdColor = Sys.Console.ForegroundColor;
Mn.Used IsUsed = Mn.Used.Not;
for (int y = 0; y < SizeY; y++)
{
Sys.Console.Write(" |");
for (int x = 0; x < SizeX; x++)
{
Sys.Console.Write(" |");
}
Sys.Console.WriteLine();
Sys.Console.Write(" |");
for (int x = 0; x < SizeX; x++)
{
IsUsed = Mn.Used.Not;
for (int ps = 0; ps < path.Length; ps++)
{
LibPF.Position p = path[ps];
if (p.X == x && p.Y == y)
{
IsUsed = (ps == 0 ? Mn.Used.Start : ((ps == (path.Length - 1)) ? Mn.Used.End : Mn.Used.Path));
break;
}
}
Sys.Console.Write(" ");
switch (IsUsed)
{
case Mn.Used.Not:
if (g.GetCellCost(x, y) == float.PositiveInfinity)
{
AUX = "xXx";
Sys.Console.ForegroundColor = Sys.ConsoleColor.Red;
}
else
{
AUX = " ";
Sys.Console.ForegroundColor = stdColor;
}
break;
case Mn.Used.Start:
AUX = "-S-";
Sys.Console.ForegroundColor = Sys.ConsoleColor.Blue;
break;
case Mn.Used.Path:
AUX = "-U-";
Sys.Console.ForegroundColor = Sys.ConsoleColor.Green;
break;
case Mn.Used.End:
AUX = "-E-";
Sys.Console.ForegroundColor = Sys.ConsoleColor.Cyan;
break;
}
Sys.Console.Write(AUX);
Sys.Console.ForegroundColor = stdColor;
Sys.Console.Write(" |");
}
Sys.Console.Write(y.ToString());
Sys.Console.WriteLine();
Sys.Console.Write(" |");
for (int x = 0; x < SizeX; x++)
{
Sys.Console.Write("_____|");
}
Sys.Console.WriteLine();
}
posTime = Sys.DateTime.Now.Ticks;
Sys.Console.WriteLine();
Sys.Console.WriteLine();
Sys.Console.WriteLine("Time (Draw Interface): " + Sys.TimeSpan.FromTicks(posTime - preTime).ToString());
}
else
{
Sys.Console.Write("Could Not Convert JSon | This is the first test to see if the Grid can be saved and loaded from files.");
}
}
