Developing a pathfinding algorithm