Fixed bug in BFS utils causing Entities to de-rail if they switch between different BFS paths when not perfectly grid-aligned