Add method to sum values to all cells within a given shape

examples/cpp/edit/sum_map.cc

@@ -4,6 +4,8 @@
 #include <wavemap/core/utils/edit/crop.h>
 #include <wavemap/core/utils/edit/sum.h>
 #include <wavemap/core/utils/edit/transform.h>
+#include <wavemap/core/utils/geometry/aabb.h>
+#include <wavemap/core/utils/geometry/sphere.h>
 #include <wavemap/io/file_conversions.h>
 
 using namespace wavemap;  // NOLINT
@@ -33,6 +35,14 @@ int main(int, char**) {
   // Merge them together
   edit::sum(*map, *map_translated);
 
+  // Set a box in the map to free
+  AABB<Point3D> box{{6.f, 6.f, -2.f}, {10.f, 10.f, 2.f}};
+  edit::sum(*map, box, -1.f, thread_pool);
+
+  // Set a sphere in the map to occupied
+  Sphere<Point3D> sphere{{8.f, 8.f, 0.f}, 1.5f};
+  edit::sum(*map, sphere, 2.f, thread_pool);
+
   // Save the map
   const std::filesystem::path output_map_path =
       home_dir / "your_map_merged.wvmp";

library/cpp/include/wavemap/core/utils/edit/impl/sum_inl.h

@@ -6,6 +6,8 @@
 #include <utility>
 
 #include "wavemap/core/indexing/index_conversions.h"
+#include "wavemap/core/utils/geometry/aabb.h"
+#include "wavemap/core/utils/iterate/grid_iterator.h"
 
 namespace wavemap::edit {
 namespace detail {
@@ -185,6 +187,33 @@ void sum(MapT& map_A, const MapT& map_B,
     thread_pool->wait_all();
   }
 }
+
+template <typename MapT, typename IndicatorFn>
+void sum(MapT& map, IndicatorFn shape, FloatingPoint update,
+         const std::shared_ptr<ThreadPool>& thread_pool) {
+  // Find the blocks that overlap with the shape
+  const FloatingPoint block_width =
+      convert::heightToCellWidth(map.getMinCellWidth(), map.getTreeHeight());
+  const FloatingPoint block_width_inv = 1.f / block_width;
+  const auto aabb = static_cast<AABB<Point3D>>(shape);
+  const Index3D block_index_min =
+      convert::pointToFloorIndex(aabb.min, block_width_inv);
+  const Index3D block_index_max =
+      convert::pointToCeilIndex(aabb.max, block_width_inv);
+  std::unordered_set<Index3D, IndexHash<3>> block_indices;
+  for (const Index3D& block_index : Grid(block_index_min, block_index_max)) {
+    block_indices.emplace(block_index);
+  }
+
+  // Add the update to all cells whose centers lie inside the shape
+  auto sampling_function = [&shape, update](const Point3D& t_A_p) {
+    if (shape.contains(t_A_p)) {
+      return update;
+    }
+    return 0.f;
+  };
+  sum(map, sampling_function, block_indices, 0, thread_pool);
+}
 }  // namespace wavemap::edit
 
 #endif  // WAVEMAP_CORE_UTILS_EDIT_IMPL_SUM_INL_H_

library/cpp/include/wavemap/core/utils/edit/sum.h

@@ -39,6 +39,10 @@ void sum(MapT& map, SamplingFn sampling_function,
 template <typename MapT>
 void sum(MapT& map_A, const MapT& map_B,
          const std::shared_ptr<ThreadPool>& thread_pool = nullptr);
+
+template <typename MapT, typename IndicatorFn>
+void sum(MapT& map, IndicatorFn shape, FloatingPoint update,
+         const std::shared_ptr<ThreadPool>& thread_pool = nullptr);
 }  // namespace wavemap::edit
 
 #include "wavemap/core/utils/edit/impl/sum_inl.h"

library/cpp/include/wavemap/core/utils/geometry/sphere.h

@@ -0,0 +1,44 @@
+#ifndef WAVEMAP_CORE_UTILS_GEOMETRY_SPHERE_H_
+#define WAVEMAP_CORE_UTILS_GEOMETRY_SPHERE_H_
+
+#include <algorithm>
+#include <string>
+#include <utility>
+
+#include "wavemap/core/common.h"
+#include "wavemap/core/utils/print/eigen.h"
+
+namespace wavemap {
+template <typename PointT>
+struct Sphere {
+  static constexpr int kDim = dim_v<PointT>;
+  using PointType = PointT;
+  using ScalarType = typename PointType::Scalar;
+
+  PointType center;
+  ScalarType radius;
+
+  Sphere() = default;
+  Sphere(const PointT& center, ScalarType radius)
+      : center(center), radius(radius) {}
+  Sphere(PointT&& center, ScalarType radius)
+      : center(std::move(center)), radius(radius) {}
+
+  operator AABB<PointT>() const {
+    return AABB<PointT>(center.array() - radius, center.array() + radius);
+  }
+
+  bool contains(const PointType& point) const {
+    return (point - center).squaredNorm() <= radius * radius;
+  }
+
+  std::string toString() const {
+    std::stringstream ss;
+    ss << "[center =" << print::eigen::oneLine(center)
+       << ", radius = " << radius << "]";
+    return ss.str();
+  }
+};
+}  // namespace wavemap
+
+#endif  // WAVEMAP_CORE_UTILS_GEOMETRY_SPHERE_H_