public void Find2Neighbors()
{
var map = new byte[3, 3] {
{ 1, 2, 3, },
{ 3, 4, 2, },
{ 5, 6, 5, },
};
var pointX = 2;
var pointY = 2;
var neighbors = NeighborsFinder.Find(map, pointX, pointY);
Assert.Equal(2, neighbors.Length);
Assert.Contains(neighbors, n => n.X == pointX - 1 && n.Y == pointY);
Assert.Contains(neighbors, n => n.X == pointX && n.Y == pointY - 1);
}
public Location[] Find(byte[,] map, Location start, Location finish)
{
if (start == finish)
{
throw new ArgumentException("Start and finish can't be the same");
}
if (start.X < 0 || start.X >= map.GetLength(0) ||
start.Y < 0 || start.Y >= map.GetLength(1))
{
throw new ArgumentException(nameof(start));
}
if (finish.X < 0 || finish.X >= map.GetLength(0) ||
finish.Y < 0 || finish.Y >= map.GetLength(1))
{
throw new ArgumentException(nameof(finish));
}
var costMap = new ushort[map.GetLength(0), map.GetLength(1)];
costMap[start.X, start.Y] = MaxPossibilitiesValue;
bool isFinish = false;
var frontier = new SimplePriorityQueue();
frontier.Insert(start, 0);
var comeFrom = new Dictionary <(int x, int y), Location>();
var cheapestPath = int.MaxValue;
var iterationsCount = 0;
while (!isFinish)
{
var currentCell = frontier.Get();
var pathCost = costMap[currentCell.Cell.X, currentCell.Cell.Y];
// if cheapest path from frontier cost more,
// than already finded cheapest way to finish -
// no more reason to continue calculating.
if (pathCost > cheapestPath)
{
isFinish = true;
}
var neighbors = NeighborsFinder.Find(map, currentCell.Cell.X, currentCell.Cell.Y);
foreach (var neighbor in neighbors)
{
var passability = map[neighbor.X, neighbor.Y];
if (passability == 0)
{
continue;
}
var neighborCost = MaxPossibilitiesValue - passability;
var cheapestCostOnNeighbor = costMap[neighbor.X, neighbor.Y];
var currentPathCost = (ushort)(pathCost + neighborCost);
if (cheapestCostOnNeighbor == 0 || currentPathCost < cheapestCostOnNeighbor)
{
costMap[neighbor.X, neighbor.Y] = currentPathCost;
comeFrom[(neighbor.X, neighbor.Y)] = currentCell.Cell;