verify/ds/range_query/kd_tree.test.cpp

View this file on GitHub
Last update: 2026-10-06 01:57:24+09:00
Problem: https://onlinejudge.u-aizu.ac.jp/problems/DSL_2_C

Depends on

Code

#define PROBLEM "https://onlinejudge.u-aizu.ac.jp/problems/DSL_2_C"

#include "../../../ds/range_query/kd_tree.hpp"

#include <algorithm>
#include <array>
#include <cassert>
#include <cstdint>
#include <vector>

#include "../../../utilities/fast_io.hpp"

namespace {

using RandomTree = m1une::ds::KdTree<long long, 3>;
using RandomPoint = RandomTree::point_type;

long long distance_squared(
    const RandomPoint& first,
    const RandomPoint& second
) {
    long long result = 0;
    for (int axis = 0; axis < 3; axis++) {
        long long difference = first[axis] - second[axis];
        result += difference * difference;
    }
    return result;
}

void fixed_test() {
    using Tree = m1une::ds::KdTree<int, 2>;
    using Point = Tree::point_type;

    Tree empty;
    Point origin = {0, 0};
    assert(empty.empty());
    assert(empty.size() == 0);
    assert(empty.range_search(origin, origin).empty());
    assert(empty.count(origin, origin) == 0);
    assert(!empty.nearest(origin));
    assert(empty.k_nearest(origin, 0).empty());

    std::vector<Point> points = {
        Point{0, 0},
        Point{2, 0},
        Point{0, 2},
        Point{0, 0},
        Point{-2, 0}
    };
    Tree tree(points);
    assert(tree.size() == 5);
    assert(tree[3] == points[3]);

    Point lower = {-1, -1};
    Point upper = {1, 1};
    std::vector<int> indices = tree.range_search(lower, upper);
    std::sort(indices.begin(), indices.end());
    assert(indices == std::vector<int>({0, 3}));
    assert(tree.count(lower, upper) == 2);

    Point query = {1, 0};
    auto nearest = tree.nearest(query);
    assert(nearest.has_value());
    assert(nearest->index == 0);
    assert(nearest->distance_squared == 1);

    std::vector<Tree::Neighbor> neighbors = tree.k_nearest(query, 3);
    assert(neighbors.size() == 3);
    Tree::Neighbor first_expected{0, 1};
    Tree::Neighbor second_expected{1, 1};
    Tree::Neighbor third_expected{3, 1};
    assert(neighbors[0] == first_expected);
    assert(neighbors[1] == second_expected);
    assert(neighbors[2] == third_expected);

    tree.build({Point{10, 20}});
    assert(tree.size() == 1);
    assert(tree.nearest(origin)->index == 0);
    assert(tree.nearest(origin)->distance_squared == 500);

    using FloatTree = m1une::ds::KdTree<double, 2>;
    using FloatPoint = FloatTree::point_type;
    FloatPoint floating_first = {-0.5, 1.25};
    FloatPoint floating_second = {2.0, 3.0};
    std::vector<FloatPoint> floating_points = {
        floating_first,
        floating_second
    };
    FloatTree floating_tree(floating_points);
    FloatPoint floating_query = {0.0, 1.0};
    auto floating_nearest = floating_tree.nearest(floating_query);
    assert(floating_nearest->index == 0);
    assert(floating_nearest->distance_squared == 0.3125L);
}

void randomized_test() {
    std::uint64_t state = 123456789123456789ULL;
    auto random = [&state]() {
        state ^= state << 7;
        state ^= state >> 9;
        return state;
    };

    for (int trial = 0; trial < 300; trial++) {
        int n = int(random() % 50);
        std::vector<RandomPoint> points(n);
        for (RandomPoint& point : points) {
            for (long long& coordinate : point) {
                coordinate = static_cast<long long>(random() % 31) - 15;
            }
        }

        RandomTree tree(points);
        assert(tree.size() == n);
        assert(tree.empty() == points.empty());
        for (int index = 0; index < n; index++) {
            assert(tree[index] == points[index]);
        }

        for (int query_index = 0; query_index < 100; query_index++) {
            RandomPoint lower;
            RandomPoint upper;
            RandomPoint query;
            for (int axis = 0; axis < 3; axis++) {
                lower[axis] = static_cast<long long>(random() % 41) - 20;
                upper[axis] = static_cast<long long>(random() % 41) - 20;
                if (upper[axis] < lower[axis]) {
                    std::swap(lower[axis], upper[axis]);
                }
                query[axis] = static_cast<long long>(random() % 41) - 20;
            }

            std::vector<int> expected_range;
            std::vector<RandomTree::Neighbor> expected_neighbors;
            for (int index = 0; index < n; index++) {
                bool inside = true;
                for (int axis = 0; axis < 3; axis++) {
                    inside &= lower[axis] <= points[index][axis]
                        && points[index][axis] < upper[axis];
                }
                if (inside) expected_range.push_back(index);
                expected_neighbors.push_back(RandomTree::Neighbor{
                    index,
                    distance_squared(points[index], query)
                });
            }

            std::vector<int> actual_range = tree.range_search(lower, upper);
            std::sort(actual_range.begin(), actual_range.end());
            assert(actual_range == expected_range);
            assert(tree.count(lower, upper) == int(expected_range.size()));

            std::sort(
                expected_neighbors.begin(),
                expected_neighbors.end(),
                [](const auto& first, const auto& second) {
                    if (first.distance_squared != second.distance_squared) {
                        return first.distance_squared < second.distance_squared;
                    }
                    return first.index < second.index;
                }
            );
            auto nearest = tree.nearest(query);
            if (n == 0) {
                assert(!nearest);
            } else {
                assert(nearest.has_value());
                assert(*nearest == expected_neighbors[0]);
            }

            int k = int(random() % (n + 1));
            std::vector<RandomTree::Neighbor> actual_neighbors =
                tree.k_nearest(query, k);
            expected_neighbors.resize(k);
            assert(actual_neighbors == expected_neighbors);
        }
    }
}

}  // namespace

int main() {
    fixed_test();
    randomized_test();

    using Tree = m1une::ds::KdTree<long long, 2>;
    using Point = Tree::point_type;
    m1une::utilities::FastInput fast_input;
    m1une::utilities::FastOutput fast_output;

    int n;
    fast_input >> n;
    std::vector<Point> points(n);
    for (Point& point : points) fast_input >> point[0] >> point[1];
    Tree tree(points);

    int query_count;
    fast_input >> query_count;
    while (query_count--) {
        long long left, right, lower, upper;
        fast_input >> left >> right >> lower >> upper;
        Point box_lower = {left, lower};
        Point box_upper = {right + 1, upper + 1};
        std::vector<int> result = tree.range_search(box_lower, box_upper);
        std::sort(result.begin(), result.end());
        for (int index : result) fast_output << index << '\n';
        fast_output << '\n';
    }
}

#line 1 "verify/ds/range_query/kd_tree.test.cpp"
#define PROBLEM "https://onlinejudge.u-aizu.ac.jp/problems/DSL_2_C"

#line 1 "ds/range_query/kd_tree.hpp"



#include <algorithm>
#include <array>
#include <cassert>
#include <cstddef>
#include <limits>
#include <optional>
#include <queue>
#include <type_traits>
#include <utility>
#include <vector>

namespace m1une {
namespace ds {

// Static orthogonal range and nearest-neighbor queries in K dimensions.
template <class T, std::size_t K>
class KdTree {
    static_assert(K > 0);
    static_assert(std::is_arithmetic_v<T>);
    static_assert(!std::is_same_v<std::remove_cv_t<T>, bool>);

   public:
    using coordinate_type = T;
    using point_type = std::array<T, K>;
    using distance_type =
        std::conditional_t<std::is_integral_v<T>, __int128_t, long double>;
    static constexpr std::size_t dimension = K;

    struct Neighbor {
        int index;
        distance_type distance_squared;

        friend bool operator==(const Neighbor&, const Neighbor&) = default;
    };

   private:
    struct Node {
        point_type minimum;
        point_type maximum;
        int point_index = -1;
        int left = -1;
        int right = -1;
        int subtree_size = 1;
    };

    std::vector<point_type> _points;
    std::vector<Node> _nodes;
    int _root = -1;
    int _node_count = 0;

    static distance_type squared_distance(
        const point_type& first,
        const point_type& second
    ) {
        distance_type result = 0;
        for (std::size_t axis = 0; axis < K; axis++) {
            distance_type difference =
                distance_type(first[axis]) - distance_type(second[axis]);
            result += difference * difference;
        }
        return result;
    }

    distance_type box_distance(int node, const point_type& point) const {
        distance_type result = 0;
        for (std::size_t axis = 0; axis < K; axis++) {
            distance_type difference = 0;
            if (point[axis] < _nodes[node].minimum[axis]) {
                difference = distance_type(_nodes[node].minimum[axis])
                    - distance_type(point[axis]);
            } else if (_nodes[node].maximum[axis] < point[axis]) {
                difference = distance_type(point[axis])
                    - distance_type(_nodes[node].maximum[axis]);
            }
            result += difference * difference;
        }
        return result;
    }

    int build_node(
        std::vector<int>& order,
        int left,
        int right
    ) {
        if (left == right) return -1;

        point_type minimum = _points[order[left]];
        point_type maximum = minimum;
        for (int position = left + 1; position < right; position++) {
            const point_type& point = _points[order[position]];
            for (std::size_t axis = 0; axis < K; axis++) {
                if (point[axis] < minimum[axis]) minimum[axis] = point[axis];
                if (maximum[axis] < point[axis]) maximum[axis] = point[axis];
            }
        }

        std::size_t split_axis = 0;
        distance_type maximum_extent =
            distance_type(maximum[0]) - distance_type(minimum[0]);
        for (std::size_t axis = 1; axis < K; axis++) {
            distance_type extent =
                distance_type(maximum[axis]) - distance_type(minimum[axis]);
            if (maximum_extent < extent) {
                maximum_extent = extent;
                split_axis = axis;
            }
        }

        int middle = (left + right) / 2;
        std::nth_element(
            order.begin() + left,
            order.begin() + middle,
            order.begin() + right,
            [&](int first, int second) {
                if (_points[first][split_axis]
                    < _points[second][split_axis]) return true;
                if (_points[second][split_axis]
                    < _points[first][split_axis]) return false;
                return first < second;
            }
        );

        int node = _node_count++;
        _nodes[node].point_index = order[middle];
        _nodes[node].minimum = minimum;
        _nodes[node].maximum = maximum;
        _nodes[node].left = build_node(order, left, middle);
        _nodes[node].right = build_node(order, middle + 1, right);
        _nodes[node].subtree_size = 1;
        if (_nodes[node].left != -1) {
            _nodes[node].subtree_size +=
                _nodes[_nodes[node].left].subtree_size;
        }
        if (_nodes[node].right != -1) {
            _nodes[node].subtree_size +=
                _nodes[_nodes[node].right].subtree_size;
        }
        return node;
    }

    static bool contains(
        const point_type& point,
        const point_type& lower,
        const point_type& upper
    ) {
        for (std::size_t axis = 0; axis < K; axis++) {
            if (point[axis] < lower[axis] || !(point[axis] < upper[axis])) {
                return false;
            }
        }
        return true;
    }

    bool disjoint(
        int node,
        const point_type& lower,
        const point_type& upper
    ) const {
        for (std::size_t axis = 0; axis < K; axis++) {
            if (_nodes[node].maximum[axis] < lower[axis]
                || !(_nodes[node].minimum[axis] < upper[axis])) {
                return true;
            }
        }
        return false;
    }

    bool covered(
        int node,
        const point_type& lower,
        const point_type& upper
    ) const {
        for (std::size_t axis = 0; axis < K; axis++) {
            if (_nodes[node].minimum[axis] < lower[axis]
                || !(_nodes[node].maximum[axis] < upper[axis])) {
                return false;
            }
        }
        return true;
    }

    void collect_subtree(int node, std::vector<int>& result) const {
        if (node == -1) return;
        result.push_back(_nodes[node].point_index);
        collect_subtree(_nodes[node].left, result);
        collect_subtree(_nodes[node].right, result);
    }

    void range_search_node(
        int node,
        const point_type& lower,
        const point_type& upper,
        std::vector<int>& result
    ) const {
        if (node == -1 || disjoint(node, lower, upper)) return;
        if (covered(node, lower, upper)) {
            collect_subtree(node, result);
            return;
        }
        int point_index = _nodes[node].point_index;
        if (contains(_points[point_index], lower, upper)) {
            result.push_back(point_index);
        }
        range_search_node(_nodes[node].left, lower, upper, result);
        range_search_node(_nodes[node].right, lower, upper, result);
    }

    int count_node(
        int node,
        const point_type& lower,
        const point_type& upper
    ) const {
        if (node == -1 || disjoint(node, lower, upper)) return 0;
        if (covered(node, lower, upper)) return _nodes[node].subtree_size;
        int result = int(contains(
            _points[_nodes[node].point_index],
            lower,
            upper
        ));
        result += count_node(_nodes[node].left, lower, upper);
        result += count_node(_nodes[node].right, lower, upper);
        return result;
    }

    static bool better(const Neighbor& first, const Neighbor& second) {
        if (first.distance_squared != second.distance_squared) {
            return first.distance_squared < second.distance_squared;
        }
        return first.index < second.index;
    }

    void nearest_node(
        int node,
        const point_type& query,
        std::optional<Neighbor>& best
    ) const {
        if (node == -1) return;
        distance_type bound = box_distance(node, query);
        if (best && best->distance_squared < bound) return;

        int point_index = _nodes[node].point_index;
        Neighbor candidate{
            point_index,
            squared_distance(_points[point_index], query)
        };
        if (!best || better(candidate, *best)) best = candidate;

        int first = _nodes[node].left;
        int second = _nodes[node].right;
        if (first != -1 && second != -1
            && box_distance(second, query) < box_distance(first, query)) {
            std::swap(first, second);
        }
        nearest_node(first, query, best);
        nearest_node(second, query, best);
    }

    using HeapEntry = std::pair<distance_type, int>;

    void k_nearest_node(
        int node,
        const point_type& query,
        int k,
        std::priority_queue<HeapEntry>& heap
    ) const {
        if (node == -1) return;
        distance_type bound = box_distance(node, query);
        if (int(heap.size()) == k && heap.top().first < bound) return;

        int point_index = _nodes[node].point_index;
        HeapEntry candidate = {
            squared_distance(_points[point_index], query),
            point_index
        };
        if (int(heap.size()) < k) {
            heap.push(candidate);
        } else if (candidate < heap.top()) {
            heap.pop();
            heap.push(candidate);
        }

        int first = _nodes[node].left;
        int second = _nodes[node].right;
        if (first != -1 && second != -1
            && box_distance(second, query) < box_distance(first, query)) {
            std::swap(first, second);
        }
        k_nearest_node(first, query, k, heap);
        k_nearest_node(second, query, k, heap);
    }

    static void assert_valid_box(
        const point_type& lower,
        const point_type& upper
    ) {
        for (std::size_t axis = 0; axis < K; axis++) {
            assert(!(upper[axis] < lower[axis]));
        }
    }

   public:
    KdTree() = default;

    explicit KdTree(const std::vector<point_type>& points) {
        build(points);
    }

    void build(const std::vector<point_type>& points) {
        assert(points.size()
            <= std::size_t(std::numeric_limits<int>::max()));
        _points = points;
        _nodes.clear();
        _nodes.resize(points.size());
        _root = -1;
        _node_count = 0;
        std::vector<int> order(points.size());
        for (int index = 0; index < int(points.size()); index++) {
            order[index] = index;
        }
        _root = build_node(order, 0, int(order.size()));
    }

    int size() const {
        return int(_points.size());
    }

    bool empty() const {
        return _points.empty();
    }

    const point_type& get(int index) const {
        assert(0 <= index && index < size());
        return _points[index];
    }

    const point_type& operator[](int index) const {
        return get(index);
    }

    std::vector<int> range_search(
        const point_type& lower,
        const point_type& upper
    ) const {
        assert_valid_box(lower, upper);
        std::vector<int> result;
        range_search_node(_root, lower, upper, result);
        return result;
    }

    int count(
        const point_type& lower,
        const point_type& upper
    ) const {
        assert_valid_box(lower, upper);
        return count_node(_root, lower, upper);
    }

    std::optional<Neighbor> nearest(const point_type& query) const {
        std::optional<Neighbor> result;
        nearest_node(_root, query, result);
        return result;
    }

    std::vector<Neighbor> k_nearest(
        const point_type& query,
        int k
    ) const {
        assert(0 <= k && k <= size());
        if (k == 0) return {};
        std::priority_queue<HeapEntry> heap;
        k_nearest_node(_root, query, k, heap);
        std::vector<Neighbor> result;
        result.reserve(k);
        while (!heap.empty()) {
            result.push_back(Neighbor{heap.top().second, heap.top().first});
            heap.pop();
        }
        std::reverse(result.begin(), result.end());
        return result;
    }
};

}  // namespace ds
}  // namespace m1une


#line 4 "verify/ds/range_query/kd_tree.test.cpp"

#line 8 "verify/ds/range_query/kd_tree.test.cpp"
#include <cstdint>
#line 10 "verify/ds/range_query/kd_tree.test.cpp"

#line 1 "utilities/fast_io.hpp"



#line 6 "utilities/fast_io.hpp"
#include <cerrno>
#include <charconv>
#line 9 "utilities/fast_io.hpp"
#include <cstdio>
#include <cstdlib>
#line 12 "utilities/fast_io.hpp"
#include <cstring>
#include <iterator>
#include <string>
#include <sys/stat.h>
#line 18 "utilities/fast_io.hpp"
#include <unistd.h>
#line 20 "utilities/fast_io.hpp"

namespace m1une {
namespace utilities {

struct FastOutput;

namespace internal {

// Shared with the convenience helpers in template.hpp.
inline FastOutput* standard_output_instance = nullptr;

// Detect std::begin(x), std::end(x).
template <class T, class = void>
struct is_range : std::false_type {};

template <class T>
struct is_range<T, std::void_t<
    decltype(std::begin(std::declval<T&>())),
    decltype(std::end(std::declval<T&>()))
>> : std::true_type {};

template <class T>
inline constexpr bool is_range_v = is_range<T>::value;

template <class T>
using range_reference_t = decltype(*std::begin(std::declval<T&>()));

template <class T>
using range_value_t = std::remove_cv_t<std::remove_reference_t<range_reference_t<T>>>;

template <class T, class = void>
struct range_stored_value {
    using type = range_value_t<T>;
};

template <class T>
struct range_stored_value<T, std::void_t<typename std::remove_cv_t<std::remove_reference_t<T>>::value_type>> {
    using type = typename std::remove_cv_t<std::remove_reference_t<T>>::value_type;
};

template <class T>
using range_stored_value_t = typename range_stored_value<T>::type;

// Treat strings and C strings as scalar output objects, not as ranges.
template <class T>
struct is_char_array : std::false_type {};

template <class T, std::size_t N>
struct is_char_array<T[N]>
    : std::bool_constant<std::is_same_v<std::remove_cv_t<T>, char>> {};

template <class T>
struct is_string_like
    : std::bool_constant<
          std::is_same_v<std::decay_t<T>, std::string>
          || std::is_same_v<std::decay_t<T>, const char*>
          || std::is_same_v<std::decay_t<T>, char*>
          || is_char_array<std::remove_reference_t<T>>::value
      > {};

template <class T>
inline constexpr bool is_string_like_v = is_string_like<T>::value;

// ModInt-like type: x.val() is printable, and x can be assigned from long long.
template <class T, class = void>
struct has_val_method : std::false_type {};

template <class T>
struct has_val_method<T, std::void_t<decltype(std::declval<const T&>().val())>>
    : std::true_type {};

template <class T>
inline constexpr bool has_val_method_v = has_val_method<T>::value;

template <class T, class = void>
struct has_static_mod_raw : std::false_type {};

template <class T>
struct has_static_mod_raw<
    T, std::void_t<decltype(T::mod()), decltype(T::raw(std::declval<uint32_t>()))>>
    : std::true_type {};

template <class T>
inline constexpr bool has_static_mod_raw_v = has_static_mod_raw<T>::value;

// libstdc++ before GCC 16 does not classify __int128 as an integral type in
// strict ISO modes such as -std=c++23. Keep the fast-I/O interface independent
// of that implementation detail.
template <class T>
inline constexpr bool is_integral_v =
    std::is_integral_v<T>
    || std::is_same_v<std::remove_cv_t<T>, __int128_t>
    || std::is_same_v<std::remove_cv_t<T>, __uint128_t>;

template <class T>
inline constexpr bool is_signed_v =
    std::is_signed_v<T>
    || std::is_same_v<std::remove_cv_t<T>, __int128_t>;

template <class T>
struct make_unsigned {
    using type = std::make_unsigned_t<T>;
};

template <>
struct make_unsigned<__int128_t> {
    using type = __uint128_t;
};

template <>
struct make_unsigned<__uint128_t> {
    using type = __uint128_t;
};

template <class T>
using make_unsigned_t = typename make_unsigned<std::remove_cv_t<T>>::type;

}  // namespace internal

struct FastInput {
    static constexpr int buffer_size = 1 << 20;

   private:
    std::FILE* _stream;
    char _buffer[buffer_size];
    int _position;
    int _length;
    int _file_descriptor;
    bool _streaming;

    bool refill() {
        _position = 0;
        if (_streaming) {
            ssize_t length;
            do {
                length = ::read(_file_descriptor, _buffer, buffer_size);
            } while (length < 0 && errno == EINTR);
            if (length <= 0) {
                _length = 0;
                return false;
            }
            _length = int(length);
        } else {
            _length = int(std::fread(_buffer, 1, buffer_size, _stream));
        }
        return _length != 0;
    }

    template <class T>
    bool read_integer_from_stream(T& value) {
        if (!skip_spaces()) return false;
        int c = read_char_raw();

        bool negative = false;
        if (c == '-') {
            negative = true;
            c = read_char_raw();
        }

        if constexpr (internal::is_signed_v<T>) {
            T result = 0;
            while ('0' <= c && c <= '9') {
                result = negative ? result * 10 - (c - '0')
                                  : result * 10 + (c - '0');
                c = read_char_raw();
            }
            value = result;
        } else {
            T result = 0;
            while ('0' <= c && c <= '9') {
                result = result * 10 + T(c - '0');
                c = read_char_raw();
            }
            value = negative ? T(0) - result : result;
        }
        return true;
    }

    bool prepare_number() {
        if (_length - _position >= 64) return true;
        const int remaining = _length - _position;
        if (remaining > 0) std::memmove(_buffer, _buffer + _position, remaining);
        const int added = int(std::fread(_buffer + remaining, 1, buffer_size - remaining, _stream));
        _position = 0;
        _length = remaining + added;
        if (_length < buffer_size) _buffer[_length] = '\0';
        return _length != 0;
    }

   public:
    explicit FastInput(std::FILE* stream = stdin)
        : _stream(stream),
          _position(0),
          _length(0),
          _file_descriptor(::fileno(stream)),
          _streaming([&] {
              struct stat status;
              return _file_descriptor >= 0
                     && ::fstat(_file_descriptor, &status) == 0
                     && !S_ISREG(status.st_mode);
          }()) {}

    FastInput(const FastInput&) = delete;
    FastInput& operator=(const FastInput&) = delete;

    int read_char_raw() {
        if (_position == _length && !refill()) return EOF;
        return _buffer[_position++];
    }

    bool skip_spaces() {
        int c = read_char_raw();
        while (c != EOF && c <= ' ') c = read_char_raw();
        if (c == EOF) return false;
        --_position;
        return true;
    }

    bool read(char& value) {
        if (!skip_spaces()) return false;
        value = char(read_char_raw());
        return true;
    }

    bool read(std::string& value) {
        if (!skip_spaces()) return false;
        value.clear();
        while (true) {
            const int begin = _position;
            while (_position < _length &&
                   static_cast<unsigned char>(_buffer[_position]) > ' ') {
                ++_position;
            }
            value.append(_buffer + begin, _position - begin);
            if (_position < _length) {
                ++_position;
                return true;
            }
            if (!refill()) return true;
        }
    }

    bool read(bool& value) {
        int x;
        if (!read(x)) return false;
        value = x != 0;
        return true;
    }

    template <class T>
    std::enable_if_t<
        internal::is_integral_v<T>
            && !std::is_same_v<std::remove_cv_t<T>, bool>
            && !std::is_same_v<std::remove_cv_t<T>, char>,
        bool
    >
    read(T& value) {
        if (_streaming) return read_integer_from_stream(value);
        if (!prepare_number()) return false;
        int c = static_cast<unsigned char>(_buffer[_position++]);
        while (c <= ' ') c = static_cast<unsigned char>(_buffer[_position++]);

        bool negative = false;
        if (c == '-') {
            negative = true;
            c = static_cast<unsigned char>(_buffer[_position++]);
        }

        if constexpr (internal::is_signed_v<T>) {
            T result = 0;
            while ('0' <= c && c <= '9') {
                const int first = c - '0';
                const int second = static_cast<unsigned char>(_buffer[_position]) - '0';
                if (0 <= second && second <= 9) {
                    result = negative ? result * 100 - (first * 10 + second)
                                      : result * 100 + (first * 10 + second);
                    ++_position;
                } else {
                    result = negative ? result * 10 - first : result * 10 + first;
                }
                c = static_cast<unsigned char>(_buffer[_position++]);
            }
            value = result;
        } else {
            T result = 0;
            while ('0' <= c && c <= '9') {
                const unsigned first = unsigned(c - '0');
                const int second = static_cast<unsigned char>(_buffer[_position]) - '0';
                if (0 <= second && second <= 9) {
                    result = result * 100 + T(first * 10 + unsigned(second));
                    ++_position;
                } else {
                    result = result * 10 + T(first);
                }
                c = static_cast<unsigned char>(_buffer[_position++]);
            }
            value = negative ? T(0) - result : result;
        }
        if (_position > _length) _position = _length;
        return true;
    }

    template <class T>
    std::enable_if_t<std::is_floating_point_v<T>, bool>
    read(T& value) {
        if (!skip_spaces()) return false;
        int c = read_char_raw();
        bool negative = false;
        if (c == '-' || c == '+') {
            negative = c == '-';
            c = read_char_raw();
        }

        long double result = 0;
        while ('0' <= c && c <= '9') {
            result = result * 10 + (c - '0');
            c = read_char_raw();
        }
        if (c == '.') {
            long double place = 0.1L;
            c = read_char_raw();
            while ('0' <= c && c <= '9') {
                result += (c - '0') * place;
                place *= 0.1L;
                c = read_char_raw();
            }
        }
        if (c == 'e' || c == 'E') {
            c = read_char_raw();
            bool exponent_negative = false;
            if (c == '-' || c == '+') {
                exponent_negative = c == '-';
                c = read_char_raw();
            }
            int exponent = 0;
            while ('0' <= c && c <= '9') {
                exponent = exponent * 10 + (c - '0');
                c = read_char_raw();
            }
            long double scale = 1;
            long double power = 10;
            while (exponent > 0) {
                if (exponent & 1) scale *= power;
                power *= power;
                exponent >>= 1;
            }
            result = exponent_negative ? result / scale : result * scale;
        }
        value = static_cast<T>(negative ? -result : result);
        return true;
    }

    template <class T>
    std::enable_if_t<
        internal::has_val_method_v<T>
            && !internal::is_integral_v<T>
            && !internal::is_range_v<T>,
        bool
    >
    read(T& value) {
        long long x;
        if (!read(x)) return false;
        if constexpr (internal::has_static_mod_raw_v<T>) {
            if (x >= 0 && uint64_t(x) < uint64_t(T::mod())) {
                value = T::raw(uint32_t(x));
            } else {
                value = T(x);
            }
        } else {
            value = T(x);
        }
        return true;
    }

    template <class First, class Second>
    bool read(std::pair<First, Second>& value) {
        if (!read(value.first)) return false;
        return read(value.second);
    }

    template <class Range>
    std::enable_if_t<
        internal::is_range_v<Range>
            && !internal::is_string_like_v<Range>,
        bool
    >
    read(Range& range) {
        using StoredValue = internal::range_stored_value_t<Range>;
        constexpr bool nested = internal::is_range_v<StoredValue>
                                && !internal::is_string_like_v<StoredValue>;

        for (auto&& value : range) {
            if constexpr (std::is_same_v<StoredValue, bool> && !nested) {
                bool x;
                if (!read(x)) return false;
                value = x;
            } else {
                if (!read(value)) return false;
            }
        }
        return true;
    }

    template <class First, class Second, class... Rest>
    bool read(First& first, Second& second, Rest&... rest) {
        if (!read(first)) return false;
        return read(second, rest...);
    }

    template <class T>
    FastInput& operator>>(T& value) {
        if (!read(value)) std::abort();
        return *this;
    }
};

struct FastOutput {
    static constexpr int buffer_size = 1 << 20;

   private:
    inline static const auto digit_quads = [] {
        std::array<char, 40000> result{};
        for (int i = 0; i < 10000; i++) {
            int value = i;
            for (int j = 3; j >= 0; j--) {
                result[4 * i + j] = char('0' + value % 10);
                value /= 10;
            }
        }
        return result;
    }();

    std::FILE* _stream;
    char _buffer[buffer_size];
    int _position;
    int _precision;
    std::chars_format _float_format;
    char _range_separator;
    std::string* _capture = nullptr;

    template <class T>
    std::string format_cell(const T& value) {
        std::string result;
        struct CaptureGuard {
            std::string*& target;
            std::string* previous;
            ~CaptureGuard() { target = previous; }
        } guard{_capture, _capture};
        _capture = &result;
        write(value);
        return result;
    }

    template <class Matrix>
    void write_aligned_matrix(const Matrix& matrix) {
        std::vector<std::vector<std::string>> rows;
        std::vector<std::size_t> widths;
        for (const auto& row : matrix) {
            auto& cells = rows.emplace_back();
            std::size_t column = 0;
            for (const auto& value : row) {
                cells.push_back(format_cell(value));
                if (column == widths.size()) widths.push_back(0);
                widths[column] = std::max(widths[column], cells.back().size());
                ++column;
            }
        }
        bool first = true;
        for (const auto& row : rows) {
            if (!first) write_char('\n');
            first = false;
            for (std::size_t column = 0; column < row.size(); ++column) {
                if (column != 0) write_char(_range_separator);
                for (std::size_t padding = row[column].size();
                     padding < widths[column]; ++padding) {
                    write_char(' ');
                }
                write(row[column]);
            }
        }
    }

   public:
    explicit FastOutput(std::FILE* stream = stdout)
        : _stream(stream),
          _position(0),
          _precision(6),
          _float_format(std::chars_format::general),
          _range_separator(' ') {
        if (_stream == stdout
            && internal::standard_output_instance == nullptr) {
            internal::standard_output_instance = this;
        }
    }

    FastOutput(const FastOutput&) = delete;
    FastOutput& operator=(const FastOutput&) = delete;

    ~FastOutput() {
        flush();
        if (internal::standard_output_instance == this) {
            internal::standard_output_instance = nullptr;
        }
    }

    void flush() {
        if (_position != 0) {
            std::fwrite(_buffer, 1, _position, _stream);
            _position = 0;
        }
        std::fflush(_stream);
    }

    void write_char(char c) {
        if (_capture != nullptr) {
            _capture->push_back(c);
            return;
        }
        if (_position == buffer_size) flush();
        _buffer[_position++] = c;
    }

    void write(const char* s) {
        while (*s != '\0') write_char(*s++);
    }

    void write(const std::string& s) {
        if (_capture != nullptr) {
            _capture->append(s);
            return;
        }
        std::size_t position = 0;
        while (position < s.size()) {
            if (_position == buffer_size) flush();
            const std::size_t copied =
                std::min<std::size_t>(buffer_size - _position, s.size() - position);
            std::memcpy(_buffer + _position, s.data() + position, copied);
            _position += int(copied);
            position += copied;
        }
    }

    void write(char c) {
        write_char(c);
    }

    void write(bool value) {
        write_char(value ? '1' : '0');
    }

    template <class T>
    std::enable_if_t<std::is_floating_point_v<T>>
    write(T value) {
        char digits[128];
        auto [end, error] = std::to_chars(
            digits,
            digits + sizeof(digits),
            value,
            _float_format,
            _precision
        );
        if (error != std::errc()) std::abort();
        for (const char* pointer = digits; pointer != end; pointer++) {
            write_char(*pointer);
        }
    }

    template <class T>
    std::enable_if_t<
        internal::is_integral_v<T>
            && !std::is_same_v<std::remove_cv_t<T>, bool>
            && !std::is_same_v<std::remove_cv_t<T>, char>
    >
    write(T value) {
        using Raw = std::remove_cv_t<T>;
        using Unsigned = internal::make_unsigned_t<Raw>;

        Unsigned magnitude;
        if constexpr (internal::is_signed_v<Raw>) {
            if (value < 0) {
                write_char('-');
                magnitude = Unsigned(0) - Unsigned(value);
            } else {
                magnitude = Unsigned(value);
            }
        } else {
            magnitude = value;
        }

        if (magnitude == 0) {
            write_char('0');
            return;
        }

        unsigned chunks[16];
        int count = 0;
        while (magnitude >= 10000) {
            const Unsigned quotient = magnitude / 10000;
            chunks[count++] = unsigned(magnitude - quotient * 10000);
            magnitude = quotient;
        }
        if (_capture == nullptr && _position > buffer_size - 64) flush();
        char captured[64];
        char* const begin = _capture != nullptr ? captured : _buffer + _position;
        char* destination = begin;
        const unsigned leading = unsigned(magnitude);
        const char* first = digit_quads.data() + 4 * leading;
        int skip = leading < 10 ? 3 : leading < 100 ? 2 : leading < 1000 ? 1 : 0;
        for (; skip < 4; skip++) *destination++ = first[skip];
        while (count--) {
            const char* digits = digit_quads.data() + 4 * chunks[count];
            std::memcpy(destination, digits, 4);
            destination += 4;
        }
        if (_capture != nullptr) {
            _capture->append(begin, destination - begin);
        } else {
            _position += int(destination - begin);
        }
    }

    template <class T>
    std::enable_if_t<
        internal::has_val_method_v<T>
            && !internal::is_integral_v<T>
            && !internal::is_range_v<T>
    >
    write(const T& value) {
        write(value.val());
    }

    template <class First, class Second>
    void write(const std::pair<First, Second>& value) {
        write(value.first);
        write_char(' ');
        write(value.second);
    }

    template <class Range>
    std::enable_if_t<
        internal::is_range_v<Range>
            && !internal::is_string_like_v<Range>
    >
    write(const Range& range) {
        using StoredValue = internal::range_stored_value_t<const Range>;
        constexpr bool nested = internal::is_range_v<StoredValue>
                                && !internal::is_string_like_v<StoredValue>;

        bool first = true;
        for (const auto& value : range) {
            if (!first) write_char(nested ? '\n' : _range_separator);
            first = false;
            if constexpr (std::is_same_v<StoredValue, bool> && !nested) {
                write(static_cast<bool>(value));
            } else {
                write(value);
            }
        }
    }

    template <class First, class... Rest>
    void print(const First& first, const Rest&... rest) {
        write(first);
        ((write_char(' '), write(rest)), ...);
    }

    void println() {
        write_char('\n');
    }

    void set_precision(int precision) {
        _precision = precision;
    }

    void set_fixed(int precision = 6) {
        _float_format = std::chars_format::fixed;
        _precision = precision;
    }

    void set_general(int precision = 6) {
        _float_format = std::chars_format::general;
        _precision = precision;
    }

    void set_range_separator(char separator) {
        _range_separator = separator;
    }

    template <class Matrix>
    void write_aligned(const Matrix& matrix) {
        using Row = internal::range_stored_value_t<const Matrix>;
        using Cell = internal::range_stored_value_t<const Row>;
        static_assert(internal::is_range_v<Row> && !internal::is_string_like_v<Row>,
                      "write_aligned requires a two-dimensional range");
        static_assert(!internal::is_range_v<Cell> || internal::is_string_like_v<Cell>,
                      "write_aligned requires scalar cells");
        write_aligned_matrix(matrix);
    }

    template <class Matrix>
    void println_aligned(const Matrix& matrix) {
        write_aligned(matrix);
        write_char('\n');
    }

    template <class... Args>
    void println(const Args&... args) {
        print(args...);
        write_char('\n');
    }

    template <class T>
    FastOutput& operator<<(const T& value) {
        write(value);
        return *this;
    }
};

}  // namespace utilities
}  // namespace m1une


#line 12 "verify/ds/range_query/kd_tree.test.cpp"

namespace {

using RandomTree = m1une::ds::KdTree<long long, 3>;
using RandomPoint = RandomTree::point_type;

long long distance_squared(
    const RandomPoint& first,
    const RandomPoint& second
) {
    long long result = 0;
    for (int axis = 0; axis < 3; axis++) {
        long long difference = first[axis] - second[axis];
        result += difference * difference;
    }
    return result;
}

void fixed_test() {
    using Tree = m1une::ds::KdTree<int, 2>;
    using Point = Tree::point_type;

    Tree empty;
    Point origin = {0, 0};
    assert(empty.empty());
    assert(empty.size() == 0);
    assert(empty.range_search(origin, origin).empty());
    assert(empty.count(origin, origin) == 0);
    assert(!empty.nearest(origin));
    assert(empty.k_nearest(origin, 0).empty());

    std::vector<Point> points = {
        Point{0, 0},
        Point{2, 0},
        Point{0, 2},
        Point{0, 0},
        Point{-2, 0}
    };
    Tree tree(points);
    assert(tree.size() == 5);
    assert(tree[3] == points[3]);

    Point lower = {-1, -1};
    Point upper = {1, 1};
    std::vector<int> indices = tree.range_search(lower, upper);
    std::sort(indices.begin(), indices.end());
    assert(indices == std::vector<int>({0, 3}));
    assert(tree.count(lower, upper) == 2);

    Point query = {1, 0};
    auto nearest = tree.nearest(query);
    assert(nearest.has_value());
    assert(nearest->index == 0);
    assert(nearest->distance_squared == 1);

    std::vector<Tree::Neighbor> neighbors = tree.k_nearest(query, 3);
    assert(neighbors.size() == 3);
    Tree::Neighbor first_expected{0, 1};
    Tree::Neighbor second_expected{1, 1};
    Tree::Neighbor third_expected{3, 1};
    assert(neighbors[0] == first_expected);
    assert(neighbors[1] == second_expected);
    assert(neighbors[2] == third_expected);

    tree.build({Point{10, 20}});
    assert(tree.size() == 1);
    assert(tree.nearest(origin)->index == 0);
    assert(tree.nearest(origin)->distance_squared == 500);

    using FloatTree = m1une::ds::KdTree<double, 2>;
    using FloatPoint = FloatTree::point_type;
    FloatPoint floating_first = {-0.5, 1.25};
    FloatPoint floating_second = {2.0, 3.0};
    std::vector<FloatPoint> floating_points = {
        floating_first,
        floating_second
    };
    FloatTree floating_tree(floating_points);
    FloatPoint floating_query = {0.0, 1.0};
    auto floating_nearest = floating_tree.nearest(floating_query);
    assert(floating_nearest->index == 0);
    assert(floating_nearest->distance_squared == 0.3125L);
}

void randomized_test() {
    std::uint64_t state = 123456789123456789ULL;
    auto random = [&state]() {
        state ^= state << 7;
        state ^= state >> 9;
        return state;
    };

    for (int trial = 0; trial < 300; trial++) {
        int n = int(random() % 50);
        std::vector<RandomPoint> points(n);
        for (RandomPoint& point : points) {
            for (long long& coordinate : point) {
                coordinate = static_cast<long long>(random() % 31) - 15;
            }
        }

        RandomTree tree(points);
        assert(tree.size() == n);
        assert(tree.empty() == points.empty());
        for (int index = 0; index < n; index++) {
            assert(tree[index] == points[index]);
        }

        for (int query_index = 0; query_index < 100; query_index++) {
            RandomPoint lower;
            RandomPoint upper;
            RandomPoint query;
            for (int axis = 0; axis < 3; axis++) {
                lower[axis] = static_cast<long long>(random() % 41) - 20;
                upper[axis] = static_cast<long long>(random() % 41) - 20;
                if (upper[axis] < lower[axis]) {
                    std::swap(lower[axis], upper[axis]);
                }
                query[axis] = static_cast<long long>(random() % 41) - 20;
            }

            std::vector<int> expected_range;
            std::vector<RandomTree::Neighbor> expected_neighbors;
            for (int index = 0; index < n; index++) {
                bool inside = true;
                for (int axis = 0; axis < 3; axis++) {
                    inside &= lower[axis] <= points[index][axis]
                        && points[index][axis] < upper[axis];
                }
                if (inside) expected_range.push_back(index);
                expected_neighbors.push_back(RandomTree::Neighbor{
                    index,
                    distance_squared(points[index], query)
                });
            }

            std::vector<int> actual_range = tree.range_search(lower, upper);
            std::sort(actual_range.begin(), actual_range.end());
            assert(actual_range == expected_range);
            assert(tree.count(lower, upper) == int(expected_range.size()));

            std::sort(
                expected_neighbors.begin(),
                expected_neighbors.end(),
                [](const auto& first, const auto& second) {
                    if (first.distance_squared != second.distance_squared) {
                        return first.distance_squared < second.distance_squared;
                    }
                    return first.index < second.index;
                }
            );
            auto nearest = tree.nearest(query);
            if (n == 0) {
                assert(!nearest);
            } else {
                assert(nearest.has_value());
                assert(*nearest == expected_neighbors[0]);
            }

            int k = int(random() % (n + 1));
            std::vector<RandomTree::Neighbor> actual_neighbors =
                tree.k_nearest(query, k);
            expected_neighbors.resize(k);
            assert(actual_neighbors == expected_neighbors);
        }
    }
}

}  // namespace

int main() {
    fixed_test();
    randomized_test();

    using Tree = m1une::ds::KdTree<long long, 2>;
    using Point = Tree::point_type;
    m1une::utilities::FastInput fast_input;
    m1une::utilities::FastOutput fast_output;

    int n;
    fast_input >> n;
    std::vector<Point> points(n);
    for (Point& point : points) fast_input >> point[0] >> point[1];
    Tree tree(points);

    int query_count;
    fast_input >> query_count;
    while (query_count--) {
        long long left, right, lower, upper;
        fast_input >> left >> right >> lower >> upper;
        Point box_lower = {left, lower};
        Point box_upper = {right + 1, upper + 1};
        std::vector<int> result = tree.range_search(box_lower, box_upper);
        std::sort(result.begin(), result.end());
        for (int index : result) fast_output << index << '\n';
        fast_output << '\n';
    }
}

m1une's library

This documentation is automatically generated by online-judge-tools/verification-helper

verify/ds/range_query/kd_tree.test.cpp

Depends on

Code