DistanceField.hpp

Go to the documentation of this file.

// Copyright 2026 Intelligent Robotics Lab
//
// This file is part of the project Easy Navigation (EasyNav in short)
// 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 NAVMAP_CORE__DISTANCEFIELD_HPP_
#define NAVMAP_CORE__DISTANCEFIELD_HPP_
 
#include <cstdint>
#include <limits>
#include <unordered_map>
#include <utility>
#include <vector>
 
#include "navmap_core/NavMap.hpp"
 
namespace navmap
{

class DistanceField
{
public:
  DistanceField(
    const NavMap & nm, bool from_boundary,
    float max_distance = std::numeric_limits<float>::infinity());

  void compute(const std::vector<NavCelId> & seed_cells);

  void update(const std::vector<NavCelId> & added, const std::vector<NavCelId> & removed);
 
  const std::vector<float> & distances() const {return dist_;}
  float operator[](NavCelId cid) const {return dist_[cid];}

  const std::vector<NavCelId> & changed() const {return changed_;}

  bool is_seed(NavCelId cid) const {return cid < seed_of_.size() && seed_of_[cid] != kNoSeed;}
 
private:
  static constexpr std::uint32_t kNoSeed = std::numeric_limits<std::uint32_t>::max();
  struct Seed
  {
    Eigen::Vector3f a, b;  
    NavCelId cell;         
    bool alive;
  };
 
  float distance_to(NavCelId c, std::uint32_t seed) const;
  void touch(NavCelId c);
  void begin_operation();
  void finish();
  template<class F>
  void flood(std::uint32_t seed, F && f);
  void splat(std::uint32_t seed);
  std::uint32_t new_point_seed(NavCelId c);
  static std::int64_t bucket_key(int bx, int by, int bz);
  void index_seed(std::uint32_t seed, bool insert);
 
  const NavMap & nm_;
  bool from_boundary_;
  float max_distance_;
  float bucket_{1.0f};
  std::unordered_map<std::int64_t, std::vector<std::uint32_t>> buckets_;  
  std::vector<Eigen::Vector3f> centroid_;
  std::vector<Seed> seeds_;                    
  std::size_t n_boundary_{0};
  std::vector<std::uint32_t> boundary_start_;  
  std::vector<std::uint32_t> seed_of_;         
  std::vector<std::uint32_t> nearest_;         
  std::vector<float> dist_;
  std::vector<std::uint32_t> free_seed_slots_;
  // Cells touched by the current operation and their previous distance
  std::vector<NavCelId> touched_;
  std::vector<float> before_;
  std::vector<std::uint32_t> touched_stamp_;
  std::uint32_t stamp_{0};
  std::vector<NavCelId> changed_;
  // Flood buffers
  std::vector<NavCelId> flood_;
  std::vector<std::uint32_t> flood_stamp_;
  std::uint32_t flood_id_{0};
};
 
}  // namespace navmap
 
#endif  // NAVMAP_CORE__DISTANCEFIELD_HPP_