Function smooth_path

pub fn smooth_path<FF, N>(
    path: &mut Vec<Vec<N>>,
    is_free: FF,
    extend_length: N,
    num_max_try: usize,
)
where
    FF: FnMut(&[N]) -> bool,
    N: Float + Debug,

select random two points, and try to connect.