Function find_path_with_goal_z 

pub fn find_path_with_goal_z(
    world: &NavWorld,
    from_x: f32,
    from_y: f32,
    from_z: f32,
    to_x: f32,
    to_y: f32,
    to_z: f32,
) -> Option<Vec<(f32, f32)>>