distance_map.hpp

Go to the documentation of this file.

// Copyright 2022-2023 Ekumen, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
#ifndef BELUGA_ALGORITHM_DISTANCE_MAP_HPP
#define BELUGA_ALGORITHM_DISTANCE_MAP_HPP
 
#include <queue>
#include <vector>
 
#include <range/v3/range/access.hpp>
#include <range/v3/range/primitives.hpp>
#include <range/v3/view/enumerate.hpp>
 
namespace beluga {
 
 
template <class Range, class DistanceFunction, class NeighborsFunction>
auto nearest_obstacle_distance_map(
    Range&& obstacle_map,
    DistanceFunction&& distance_function,
    NeighborsFunction&& neighbors_function) {
  struct IndexPair {
    std::size_t nearest_obstacle_index;
    std::size_t index;
  };
 
  using DistanceType = std::invoke_result_t<DistanceFunction, std::size_t, std::size_t>;
  auto distance_map = std::vector<DistanceType>(ranges::size(obstacle_map));
  auto visited = std::vector<bool>(ranges::size(obstacle_map), false);
 
  auto compare = [&distance_map](const IndexPair& first, const IndexPair& second) {
    return distance_map[first.index] > distance_map[second.index];
  };
  auto queue = std::priority_queue<IndexPair, std::vector<IndexPair>, decltype(compare)>{compare};
 
  for (auto [index, is_obstacle] : ranges::views::enumerate(obstacle_map)) {
    if (is_obstacle) {
      visited[index] = true;
      distance_map[index] = 0;
      queue.push(IndexPair{index, index});  // The nearest obstacle is itself
    }
  }
 
  while (!queue.empty()) {
    auto parent = queue.top();
    queue.pop();
    for (const std::size_t index : neighbors_function(parent.index)) {
      if (!visited[index]) {
        visited[index] = true;
        distance_map[index] = distance_function(parent.nearest_obstacle_index, index);
        queue.push(IndexPair{parent.nearest_obstacle_index, index});
      }
    }
  }
 
  return distance_map;
}
 
}  // namespace beluga
 
#endif