Improve intersection tests

ethz-asl · Dec 18, 2024 · e1384ba · e1384ba
1 parent 4d6d707
commit e1384ba
Show file tree

Hide file tree

Showing 2 changed files with 37 additions and 9 deletions.
diff --git a/library/cpp/include/wavemap/core/utils/edit/impl/crop_inl.h b/library/cpp/include/wavemap/core/utils/edit/impl/crop_inl.h
@@ -24,7 +24,7 @@ void cropLeavesBatch(typename MapT::Block::OctreeType::NodeRefType node,
     const Point3D t_W_child =
         convert::nodeIndexToCenterPoint(child_index, min_cell_width);
     if (!shape::is_inside(t_W_child, shape)) {
-      child_values[child_idx] = 0;
+      child_values[child_idx] = 0.f;
       if (0 < child_index.height) {
         node.eraseChild(child_idx);
       }
@@ -54,23 +54,23 @@ void cropNodeRecursive(typename MapT::Block::OctreeType::NodeRefType node,
   for (NdtreeIndexRelativeChild child_idx = 0;
        child_idx < OctreeIndex::kNumChildren; ++child_idx) {
     // If the node is fully inside the cropping shape, do nothing
-    const auto child_aabb =
-        convert::nodeIndexToAABB(node_index, min_cell_width);
+    const OctreeIndex child_index = node_index.computeChildIndex(child_idx);
+    const AABB<Point3D> child_aabb =
+        convert::nodeIndexToAABB(child_index, min_cell_width);
     if (shape::is_inside(child_aabb, shape)) {
       continue;
     }
 
     // If the node is fully outside the cropping shape, set it to zero
     auto& child_value = child_values[child_idx];
     if (!shape::overlaps(child_aabb, shape)) {
-      child_value = 0;
+      child_value = 0.f;
       node.eraseChild(child_idx);
       continue;
     }
 
     // Otherwise, continue at a higher resolution
     NodeRefType child_node = node.getOrAllocateChild(child_idx);
-    const OctreeIndex child_index = node_index.computeChildIndex(child_idx);
     if (child_index.height <= termination_height + 1) {
       cropLeavesBatch<MapT>(child_node, child_index, child_value, shape,
                             min_cell_width);

diff --git a/library/cpp/include/wavemap/core/utils/shape/intersection_tests.h b/library/cpp/include/wavemap/core/utils/shape/intersection_tests.h
@@ -5,16 +5,44 @@
 #include "wavemap/core/utils/shape/sphere.h"
 
 namespace wavemap::shape {
+// Point inside Shape
 template <typename ShapeT, int dim>
-bool is_inside(const Point<dim>& point, const ShapeT& shape) {
-  return shape.contains(point);
+bool is_inside(const Point<dim>& inner, const ShapeT& outer) {
+  return outer.contains(inner);
 }
 
+// AABB inside AABB
 template <typename PointT>
-bool is_inside(const AABB<PointT>& aabb, const Sphere<PointT>& sphere) {
-  return sphere.contains(aabb.min) && sphere.contains(aabb.max);
+bool is_inside(const AABB<PointT>& inner, const AABB<PointT>& outer) {
+  return (outer.min.array() <= inner.min.array() &&
+          inner.max.array() <= outer.max.array())
+      .all();
 }
 
+// AABB inside Sphere
+template <typename PointT>
+bool is_inside(const AABB<PointT>& inner, const Sphere<PointT>& outer) {
+  const PointT furthest_point_in_aabb = inner.furthestPointFrom(outer.center);
+  return outer.contains(furthest_point_in_aabb);
+}
+
+// Sphere inside AABB
+template <typename PointT>
+bool is_inside(const Sphere<PointT>& inner, const AABB<PointT>& outer) {
+  return (outer.min.array() <= inner.center.array() - inner.radius &&
+          inner.center.array() + inner.radius <= outer.max.array())
+      .all();
+}
+
+// AABB <-> AABB overlap
+template <typename PointT>
+bool overlaps(const AABB<PointT>& aabb_A, const AABB<PointT>& aabb_B) {
+  const auto axis_separated = aabb_A.max.array() < aabb_B.min.array() ||
+                              aabb_B.max.array() < aabb_A.min.array();
+  return !axis_separated.any();
+}
+
+// AABB <-> Sphere overlap
 template <typename PointT>
 bool overlaps(const AABB<PointT>& aabb, const Sphere<PointT>& sphere) {
   const PointT closest_point_in_aabb = aabb.closestPointTo(sphere.center);