pub fn breadth_first_search( graph: &dyn Grid3D, start: Point3D, goal: Point3D, ) -> Map<Point3D, Option<Point3D>>