Closest Point about bvh HOT 4 CLOSED

template <typename Scalar>
Scalar distance(const bvh::Vec3<Scalar>& p, const bvh::BoundingBox<Scalar>& bbox) {
    // TODO: Return the distance between the point `p` and the bounding box `bbox`
}

template <typename Bvh, typename Scalar>
std::pair<bvh::Vec3<Scalar>, Scalar> traverse(const Bvh& bvh, const bvh::Vec3<Scalar>& p) {
    std::stack<Bvh::Index> stack;
    Scalar d = std::numeric_limits<Scalar>::max();
    stack.push(0);
    while (!stack.empty()) {
        auto top = stack.top();
        stack.pop();
        if (bvh.nodes[top].is_leaf()) {
            // TODO: Find the closest point within this leaf
            // d = ...
        } else {
            auto first_child = bvh.nodes[top].first_child_or_primitive;
            auto dist_left  = distance(p, bvh.nodes[first_child + 0].bounding_box_proxy());
            auto dist_right = distance(p, bvh.nodes[first_child + 1].bounding_box_proxy());
            if (dist_left < d) {
                Bvh::Index order = 0;
                if (dist_right < d) {
                    order = dist_left > dist_right ? 1 : 0;
                    stack.push(first_child + 1 - order);
                }
                stack.push(first_child + order);
            } else if (dist_right < d) {
                stack.push(first_child + 1);
            }
        }
    }
}