public void FindPathFirstTest()
{
SquarePolygon pointToGo = ((11, 5), 2);
Index pointA = (28, 28);
SquarePolygon wall1 = ((19, 8), 4);
SquarePolygon wall2 = ((11, 17), 4);
SquarePolygon wall3 = ((23, 17), 4);
var graph = new PathGraph(30, 30);
graph.AddWalls(wall1, wall2, wall3);
graph.FindPathFirst(pointToGo, pointA);
graph.FindPathFirst(pointToGo, pointA);
graph.FindPathFirst(pointToGo, pointA);
Debug.WriteLine(graph.ToPath());
}
