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
}
}
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 SysClG.IEnumerable <LibPF.Offset> GetMovementOptions(LibPF.Position position, int dimX, int dimY, SysClG.IEnumerable <LibPF.Offset> movementPattern)
{
LibPF.Position target = default(LibPF.Position);
foreach (LibPF.Offset m in movementPattern)
{
target = position + m;
if (target.X >= 0 && target.X < dimX && target.Y >= 0 && target.Y < dimY)
{
yield return(m);
}
}
}
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 LibPF.Position[] GetPath(LibPF.Position start, LibPF.Position end, LibPF.Offset[] movementPattern, int iterationLimit)
{
SysClG.List <LibPF.Position> current = null;
using (LibPF.Finder finder = new LibPF.Finder()) { current = finder.FindPath(this, start, end, movementPattern, iterationLimit); }
if (current == null)
{
return(new LibPF.Position[0]);
}
else
{
LibPF.Position[] ar = new LibPF.Position[current.Count];
int idx = current.Count - 1;
foreach (LibPF.Position step in current)
{
ar[idx] = step;
idx--;
}
return(ar);
}
}
/* https://www.dotnetperls.com/aggressiveinlining , https://stackoverflow.com/questions/43060376/aggressiveinlining-doesnt-exist */ [SysCoS.MethodImpl(256)] private float ManhattanDistance(LibPF.Position p0, LibPF.Position p1)
{
return(Sys.Math.Abs(p0.X - p1.X) + Sys.Math.Abs(p0.Y - p1.Y));
}
private void MessageCurrent(LibPF.Position position, SysClG.List <LibPF.Position> path)
{
this.StepList.Add(new LibPF.Finder.Step(LibPF.Finder.StepType.Current, position, path));
}
private void MessageClose(LibPF.Position position)
{
this.StepList.Add(new Step(LibPF.Finder.StepType.Close, position, new SysClG.List <LibPF.Position>(0)));
}
private void MessageOpen(LibPF.Position position)
{
this.StepList.Add(new LibPF.Finder.Step(LibPF.Finder.StepType.Open, position, new SysClG.List <LibPF.Position>(0)));
}
public float GetCellCost(LibPF.Position position)
{
return(this.GetCellCost(position.X, position.Y));
}
public LibPF.Position[] GetPath(LibPF.Position start, LibPF.Position end, LibPF.Offset[] movementPattern)
{
return(this.GetPath(start, end, movementPattern, int.MaxValue));
}
public LibPF.Position[] GetPath(LibPF.Position start, LibPF.Position end)
{
return(this.GetPath(start, end, LibPF.MovementPatterns.Full));
}
internal float GetCellCostUnchecked(LibPF.Position position)
{
return(this.Weights[this.GetIndexUnchecked(position.X, position.Y)]);
}
public void UnblockCell(LibPF.Position position)
{
this.SetCellCost(position.X, position.Y, this.DefaultCost);
}
public void BlockCell(LibPF.Position position)
{
this.BlockCell(position.X, position.Y);
}
public Step(LibPF.Finder.StepType type, LibPF.Position position, SysClG.List <LibPF.Position> path)
{
this.Type = type;
this.Position = position;
this.Path = path;
}
public MinHeapNode(LibPF.Position position, float expectedCost)
{
this.Position = position;
this.ExpectedCost = expectedCost;
}
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.");
}
}
public void SetCellCost(LibPF.Position position, float cost)
{
this.SetCellCost(position.X, position.Y, cost);
}
