is nearest to a given point on a costmap using breadth-first search
line segment is in collision with the costmap or not