Function astar 

pub fn astar<F>(
    navmesh: &Navmesh,
    start: Vec3,
    end: Vec3,
    heuristic: F,
) -> Option<Vec<Waypoint>>
where
    F: Fn(Vec3, Vec3) -> f32,