/// <summary>
/// Method that switfly finds the best path from start to end. Doesn't reverse outcome
/// </summary>
/// <returns>The end breadcrump where each .next is a step back)</returns>
private static SearchNode FindPathReversed(World world, Point3D start, Point3D end)
{
SearchNode startNode = new SearchNode(start, 0, 0, null);
MinHeap openList = new MinHeap();
openList.Add(startNode);
int sizeX = world.WorldSizeX;
int sizeY = world.WorldSizeY;
int sizeZ = world.WorldSizeZ;
bool[] brWorld = new bool[sizeX * sizeY * sizeZ];
brWorld[start.X + (start.Y + start.Z * sizeY) * sizeX] = true;
while (openList.HasNext())
{
SearchNode current = openList.ExtractFirst();
if (current.position.GetDistanceSquared(end) <= 3)
{
return(new SearchNode(end, current.pathCost + 1, current.cost + 1, current));
}
for (int i = 0; i < surrounding.Length; i++)
{
Surr surr = surrounding[i];
Point3D tmp = new Point3D(current.position, surr.Point);
int brWorldIdx = tmp.X + (tmp.Y + tmp.Z * sizeY) * sizeX;
if (world.PositionIsFree(tmp) && brWorld[brWorldIdx] == false)
{
brWorld[brWorldIdx] = true;
int pathCost = current.pathCost + surr.Cost;
int cost = pathCost + tmp.GetDistanceSquared(end);
SearchNode node = new SearchNode(tmp, cost, pathCost, current);
openList.Add(node);
}
}
}
return(null); //no path found
}