sfml wave path finding in csharp

main.cs
using SFML.Graphics;
using SFML.System;
using System.Collections.Generic;
using System.Linq;

public class Node
{
    public bool IsTraversable; // is the node passable?
    public Vector2i Position;
    public int Value; // value in the wavefront grid
    public Node(int x, int y, bool traversable)
    {
        Position = new Vector2i(x, y);
        IsTraversable = traversable;
    }
}
public class SFMLWavePathFinding
{
    private readonly RenderWindow _window;
    private readonly int[,] _grid;
    private readonly List<Node> _nodeGrid = new List<Node>();
    private readonly int _gridWidth;
    private readonly int _gridHeight;
    private readonly RectangleShape _rect;
    private readonly Vector2i _startPos;
    private readonly Vector2i _endPos;

    public SFMLWavePathFinding(int[,] grid, int width, int height, Vector2i startPos, Vector2i endPos)
    {
        _window = new RenderWindow(new VideoMode((uint)width, (uint)height), "SFML Wave Path Finding");
        _rect = new RectangleShape(new Vector2f(width / grid.GetLength(0), height / grid.GetLength(1)));
        _rect.OutlineThickness = -0.5f;
        _rect.OutlineColor = Color.Black;
        _startPos = startPos;
        _endPos = endPos;
        _grid = grid;
        _gridWidth = grid.GetLength(0);
        _gridHeight = grid.GetLength(1);
        for (int x = 0; x < _gridWidth; x++)
        {
            for (int y = 0; y < _gridHeight; y++)
            {
                var traversable = _grid[x, y] != -1;
                var node = new Node(x, y, traversable);
                _nodeGrid.Add(node);
            }
        }
    }

    public void Draw()
    {
        _window.Clear(Color.White);
        foreach (var node in _nodeGrid)
        {
            _rect.FillColor = Color.White;
            if (!node.IsTraversable)
            {
                _rect.FillColor = Color.Black;
            }

            if (node.Position == _startPos)
            {
                _rect.FillColor = Color.Green;
            }
            if (node.Position == _endPos)
            {
                _rect.FillColor = Color.Red;
            }
            _rect.Position = new Vector2f(node.Position.X * _rect.Size.X, node.Position.Y * _rect.Size.Y);

            _window.Draw(_rect);
        }
        _window.Display();
    }

    public int[,] ComputeWavefront()
    {
        var wavefront = new int[_gridWidth, _gridHeight];
        var frontier = new List<Vector2i> { _endPos };
        wavefront[_endPos.X, _endPos.Y] = 0;
        var i = 0;
        while (frontier.Count > 0)
        {
            var nextFrontier = new List<Vector2i>();
            foreach (var pos in frontier)
            {
                foreach (var neighbor in GetNeighbors(pos))
                {
                    if (wavefront[neighbor.X, neighbor.Y] != 0) continue;

                    wavefront[neighbor.X, neighbor.Y] = i + 1;
                    nextFrontier.Add(neighbor);
                }
            }
            frontier = nextFrontier;
            i++;
        }
        return wavefront;
    }

    private List<Vector2i> GetNeighbors(Vector2i position)
    {
        var neighbors = new List<Vector2i>();
        for (int x = -1; x <= 1; x++)
        {
            for (int y = -1; y <= 1; y++)
            {
                if (x == 0 && y == 0) continue;
                var newPosition = position + new Vector2i(x, y);
                if (newPosition.X >= 0 && newPosition.Y >= 0 && newPosition.X < _gridWidth && newPosition.Y < _gridHeight)
                {
                    var index = _grid[newPosition.X, newPosition.Y];
                    if (index != -1)
                    {
                        var node = _nodeGrid[index];
                        if (node.IsTraversable)
                        {
                            neighbors.Add(node.Position);
                        }
                    }
                }
            }
        }
        return neighbors;
    }

    public List<Vector2i> FindPath()
    {
        var wavefront = ComputeWavefront();
        var path = new List<Vector2i> { _startPos };
        var endValue = wavefront[_startPos.X, _startPos.Y];
        while (path.Last() != _endPos)
        {
            var neighbors = GetNeighbors(path.Last());
            var bestNeighbor = neighbors[0];
            foreach (var neighbor in neighbors)
            {
                if (wavefront[neighbor.X, neighbor.Y] < wavefront[bestNeighbor.X, bestNeighbor.Y])
                {
                    bestNeighbor = neighbor;
                }
            }

            path.Add(bestNeighbor);
        }
        return path;
    }

    public void Run()
    {
        _window.SetFramerateLimit(10);
        while (_window.IsOpen)
        {
            Draw();
            _window.DispatchEvents();
        }
    }
}

4847 chars
160 lines