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 index d230bd819..5cdf2aa89 100644 --- 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,8 +54,9 @@ 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 child_aabb = + convert::nodeIndexToAABB(child_index, min_cell_width); if (shape::is_inside(child_aabb, shape)) { continue; } @@ -63,14 +64,13 @@ void cropNodeRecursive(typename MapT::Block::OctreeType::NodeRefType node, // 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(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 index df3fc7ce1..dde6944c9 100644 --- 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 -bool is_inside(const Point& point, const ShapeT& shape) { - return shape.contains(point); +bool is_inside(const Point& inner, const ShapeT& outer) { + return outer.contains(inner); } +// AABB inside AABB template -bool is_inside(const AABB& aabb, const Sphere& sphere) { - return sphere.contains(aabb.min) && sphere.contains(aabb.max); +bool is_inside(const AABB& inner, const AABB& outer) { + return (outer.min.array() <= inner.min.array() && + inner.max.array() <= outer.max.array()) + .all(); } +// AABB inside Sphere +template +bool is_inside(const AABB& inner, const Sphere& outer) { + const PointT furthest_point_in_aabb = inner.furthestPointFrom(outer.center); + return outer.contains(furthest_point_in_aabb); +} + +// Sphere inside AABB +template +bool is_inside(const Sphere& inner, const AABB& outer) { + return (outer.min.array() <= inner.center.array() - inner.radius && + inner.center.array() + inner.radius <= outer.max.array()) + .all(); +} + +// AABB <-> AABB overlap +template +bool overlaps(const AABB& aabb_A, const AABB& 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 bool overlaps(const AABB& aabb, const Sphere& sphere) { const PointT closest_point_in_aabb = aabb.closestPointTo(sphere.center);