/// <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 BreadCrumb FindPathReversed(World world, Point3D start, Point3D end)
{
MinHeap<BreadCrumb> openList = new MinHeap<BreadCrumb>(256);
BreadCrumb[, ,] brWorld = new BreadCrumb[world.Right, world.Top, world.Back];
BreadCrumb node;
Point3D tmp;
int cost;
int diff;
int count;
BreadCrumb current = new BreadCrumb(start);
current.cost = 0;
BreadCrumb finish = new BreadCrumb(end);
if (current.position.X < 0 || current.position.X > brWorld.GetUpperBound(0) || current.position.Y < 0 || current.position.Y > brWorld.GetUpperBound(0)) return null;
brWorld[current.position.X, current.position.Y, current.position.Z] = current;
openList.Add(current);
count = 0;
while (openList.Count > 0)
{
//Find best item and switch it to the 'closedList'
current = openList.ExtractFirst();
current.onClosedList = true;
//Find neighbours
for (int i = 0; i < surrounding.Length; i++)
{
tmp = current.position + surrounding[i];
if (world.PositionIsFree(tmp) || (tmp.X==end.X && tmp.Y==end.Y && tmp.Z==end.Z))
{
//Check if we've already examined a neighbour, if not create a new node for it.
if (brWorld[tmp.X, tmp.Y, tmp.Z] == null)
{
node = new BreadCrumb(tmp);
brWorld[tmp.X, tmp.Y, tmp.Z] = node;
}
else
{
node = brWorld[tmp.X, tmp.Y, tmp.Z];
}
//If the node is not on the 'closedList' check it's new score, keep the best
if (!node.onClosedList)
{
diff = 0;
if (current.position.X != node.position.X)
{
diff += 1;
}
if (current.position.Y != node.position.Y)
{
diff += 1;
}
if (current.position.Z != node.position.Z)
{
diff += 1;
}
cost = current.cost + diff + node.position.GetDistanceSquared(end);
if (cost < node.cost)
{
node.cost = cost;
node.next = current;
}
//If the node wasn't on the openList yet, add it
if (!node.onOpenList)
{
//Check to see if we're done
if (node.Equals(finish))
{
node.next = current;
return node;
}
node.onOpenList = true;
openList.Add(node);
}
}
}
}
count++;
if (count > 1000) return null;
}
return null; //no path found
}