// Search for the start and end points.
// Done together for efficiency.
private void FindStartAndEnd()
{
Vector2Int startPos = new Vector2Int(), endPos = new Vector2Int();
bool foundStart = false, foundEnd = false;
for (int row = 0; row < _map.GetLength(0); row++)
{
if (foundStart && foundEnd)
{
break;
}
for (int col = 0; col < _map.GetLength(1); col++)
{
if (foundStart && foundEnd)
{
break;
}
var current = _map[row, col];
if (current.Equals('S'))
{
startPos = new Vector2Int(col, row);
foundStart = true;
}
else if (current.Equals('E'))
{
endPos = new Vector2Int(col, row);
foundEnd = true;
}
}
}
if (!foundStart || !foundEnd)
{
throw new Exception("No start and end points!");
}
_end = new WeighedBlock(endPos, null);
_start = new WeighedBlock(startPos, _end);
}
// Creates WeighedBlocks only for the positions checked.
public void FindPath()
{
FindStartAndEnd();
// Now we need to weigh the blocks starting from the Start block
_queue.SortedAddFront(_start);
bool atEnd = false;
while (_queue.Count > 0)
{
/* For debugging purposes.
* Console.Clear();
* Console.WriteLine($"Cleared screen! {_queue.Count}");
* foreach (var b in _queue)
* {
* var prev = b.PrevBlock != null ? b.PrevBlock.Position.ToString() : "null";
* Console.WriteLine($"Pos: {b.Position}, Prev: {prev}, Step: {b.StepWeight}, Dist: {b.DistanceWeight}, Total: {b.TotalWeight}");
* }
* /**/
var block = _queue.PopFront();
// Now we get the surrounding blocks
var positions = block.GetSurroundingLocations();
var sizeX = _map.GetLength(1);
var sizeY = _map.GetLength(0);
foreach (var pos in positions)
{
// Skips the positions outside our map
if (pos.x < 0 || pos.x >= sizeX)
{
continue;
}
if (pos.y < 0 || pos.y >= sizeY)
{
continue;
}
// Skips the previous block that this block is linked to
if (block.PrevBlock != null && pos == block.PrevBlock.Position)
{
continue;
}
// Skips walls
var mapValue = _map[pos.y, pos.x];
if (mapValue.Equals('W'))
{
continue;
}
// Breaks out of this for loop when one of the neighboring
// blocks is the end block.
if (mapValue.Equals('E'))
{
Console.WriteLine("Found end!\n");
_end.StepWeight = block.StepWeight + 1;
_end.PrevBlock = block;
atEnd = true;
break;
}
// Skips if the new block has already been processed
// or is in the queue of being processed.
var newBlock = new WeighedBlock(pos, _end);
if (_finishedBlocks.Contains(newBlock))
{
continue;
}
if (_queue.Contains(newBlock))
{
continue;
}
newBlock.StepWeight = block.StepWeight + 1;
newBlock.PrevBlock = block;
_queue.SortedAddFront(newBlock);
}
if (atEnd)
{
break;
}
_finishedBlocks.Add(block);
}
TracePath();
}