AABB.h

/*
 * Copyright 2025 Adobe. All rights reserved.
 * This file is licensed to you 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 REPRESENTATIONS
 * OF ANY KIND, either express or implied. See the License for the specific language
 * governing permissions and limitations under the License.
 */
#pragma once
 
#include <lagrange/utils/function_ref.h>
#include <lagrange/utils/invalid.h>
#include <lagrange/utils/span.h>
 
#include <Eigen/Geometry>
#include <functional>
#include <vector>
 
namespace lagrange::bvh {


template <typename Scalar, int Dim>
class AABB
{
public:
    using Index = uint32_t;
    using Box = Eigen::AlignedBox<Scalar, Dim>;
    using Point = typename Box::VectorType;
 
public:
    void build(span<Box> boxes);

    void intersect(const Box& query_box, std::vector<Index>& results) const;

    void intersect(const Box& query_box, function_ref<bool(Index)> callback) const;

    Index intersect_first(const Box& query_box) const;

    Index get_closest_element(const Point& q, function_ref<Scalar(Index)> sq_dist_fn) const;

    void foreach_element_within_radius(
        const Point& q,
        Scalar sq_radius,
        function_ref<void(Index)> func) const;

    bool empty() const { return m_root == invalid<Index>(); }
 
private:
    struct Node
    {
        Box bbox;
        Index left = invalid<Index>();
        Index right = invalid<Index>();
        Index element_idx = invalid<Index>();
 
        bool is_leaf() const { return left == invalid<Index>() && right == invalid<Index>(); }
    };
 
private:
    std::vector<Node> m_nodes;
    Index m_root = invalid<Index>();
};

 
} // namespace lagrange::bvh