test/kd_tree.test.cpp

test/kd_tree.test.cpp · GitHub で見る

依存ライブラリ

コード

#define PROBLEM "https://judge.yosupo.jp/problem/closest_pair"
#include "../data_structure/kd_tree.hpp"
#include "../components/fast_io.hpp"
#include <cstdint>
#include <limits>
#include <vector>

int main() {
	FastInput input;
	FastOutput output;
	int cases = 0;
	input.read(cases);
	while (cases--) {
		int n = 0;
		input.read(n);
		using Tree = KDTree<std::int64_t, 2, std::int64_t>;
		std::vector<Tree::point_type> points(n);
		for (auto& point : points) input.read(point[0]), input.read(point[1]);
		Tree tree(points);
		std::int64_t distance = std::numeric_limits<std::int64_t>::max();
		int a = -1, b = -1;
		for (int i = 0; i < n; ++i) {
			auto nearest = tree.nearest(points[i], i);
			if (nearest->distance_squared < distance) {
				distance = nearest->distance_squared;
				a = i;
				b = nearest->index;
			}
		}
		output.write(a);
		output.write(' ');
		output.println(b);
	}
}

#line 1 "test/kd_tree.test.cpp"
#define PROBLEM "https://judge.yosupo.jp/problem/closest_pair"
#line 1 "data_structure/kd_tree.hpp"



#line 1 "components/d_ary_heap.hpp"



#include <algorithm>
#include <cassert>
#include <cstddef>
#include <functional>
#include <utility>
#include <vector>

template <class T, class Compare = std::less<T>, std::size_t D = 2>
class DaryHeap {
	static_assert(D >= 2);
	std::vector<T> data;
	[[no_unique_address]] Compare compare{};
	void up(std::size_t i, T value) {
		while (i) {
			std::size_t p = (i - 1) / D;
			if (!compare(data[p], value)) break;
			data[i] = std::move(data[p]);
			i = p;
		}
		data[i] = std::move(value);
	}
	void down(std::size_t i, T value) {
		std::size_t n = data.size();
		while (i < n / D + (n % D > 1)) {
			std::size_t best = i * D + 1, end = n;
			if constexpr (D <= std::size_t(-1) / 2) end = std::min(best + D, n);
			for (std::size_t j = best + 1; j < end; ++j)
				if (compare(data[best], data[j])) best = j;
			if (!compare(value, data[best])) break;
			data[i] = std::move(data[best]);
			i = best;
		}
		data[i] = std::move(value);
	}

  public:
	DaryHeap() = default;
	explicit DaryHeap(Compare compare) : compare(std::move(compare)) {}
	explicit DaryHeap(std::vector<T> values, Compare compare = {}) : data(std::move(values)), compare(std::move(compare)) {
		for (std::size_t i = data.size() / D + (data.size() % D > 1); i--;) down(i, std::move(data[i]));
	}
	bool empty() const { return data.empty(); }
	std::size_t size() const { return data.size(); }
	void reserve(std::size_t n) { data.reserve(n); }
	void clear() { data.clear(); }
	const std::vector<T> &values() const { return data; }
	decltype(auto) top() const {
		assert(!empty());
		return data.front();
	}
	void push(T value) {
		data.push_back(std::move(value));
		up(data.size() - 1, std::move(data.back()));
	}
	T replace_top(T value) {
		assert(!empty());
		T result = std::move(data.front());
		down(0, std::move(value));
		return result;
	}
	T push_pop(T value) {
		if (empty() || !compare(value, data.front())) return value;
		return replace_top(std::move(value));
	}
	T pop_value() {
		assert(!empty());
		T result = std::move(data.front());
		if (data.size() == 1) data.pop_back();
		else {
			T value = std::move(data.back());
			data.pop_back();
			down(0, std::move(value));
		}
		return result;
	}
	void pop() { pop_value(); }
	void swap(DaryHeap &other) noexcept(noexcept(std::swap(compare, other.compare))) {
		data.swap(other.data);
		std::swap(compare, other.compare);
	}
};


#line 6 "data_structure/kd_tree.hpp"
#include <array>
#line 8 "data_structure/kd_tree.hpp"
#include <cmath>
#line 10 "data_structure/kd_tree.hpp"
#include <cstdint>
#include <limits>
#include <optional>
#include <span>
#include <type_traits>
#line 17 "data_structure/kd_tree.hpp"

template<class T = std::int64_t, int Dimension = 2,
	class Distance = std::conditional_t<std::is_integral_v<T>, __int128_t, T>>
class KDTree {
	static_assert(Dimension > 0);
	static_assert(std::is_arithmetic_v<T> && !std::is_same_v<T, bool>);
	static_assert(!std::is_integral_v<T> || sizeof(T) <= 8);
	static_assert(std::numeric_limits<Distance>::is_bounded && !std::is_same_v<Distance, bool>);
	static_assert(std::numeric_limits<Distance>::is_integer == std::is_integral_v<T>);
	static_assert(std::numeric_limits<Distance>::is_integer ? sizeof(Distance) <= 16 :
		(std::is_floating_point_v<Distance> && std::numeric_limits<Distance>::digits >= std::numeric_limits<T>::digits));
  public:
	using point_type = std::array<T, Dimension>;
	using distance_type = Distance;
	struct neighbor_type {
		int index;
		Distance distance_squared;
		bool operator==(const neighbor_type&) const = default;
	};

  private:
	using Wide = std::conditional_t<std::is_integral_v<T>, std::conditional_t<(sizeof(T) < 8), std::int64_t, __int128_t>, Distance>;
	struct Entry { point_type point; int index; };
	struct Node { point_type lower, upper; int begin, end, minimum; std::uint32_t right = 0; };
	struct Less {
		bool operator()(const neighbor_type& a, const neighbor_type& b) const {
			return a.distance_squared < b.distance_squared || (a.distance_squared == b.distance_squared && a.index < b.index);
		}
	};
	using Heap = DaryHeap<neighbor_type, Less, 4>;
	std::vector<Entry> points_;
	std::vector<int> positions_;
	std::vector<Node> nodes_;
	int leaf_size_ = 16;
	static void check_point([[maybe_unused]] const point_type& point) {
		if constexpr (std::is_floating_point_v<T>)
			for ([[maybe_unused]] T value : point) assert(std::isfinite(value));
	}
	void check_exclude([[maybe_unused]] int exclude) const { assert(-1 <= exclude && exclude < size()); }
	static Distance square_difference(T a, T b) {
		Wide difference = Wide(a) - Wide(b);
		Distance value = Distance(difference);
		if constexpr (std::numeric_limits<Distance>::is_integer && sizeof(Distance) < sizeof(unsigned))
			return Distance(unsigned(value) * unsigned(value));
		return value * value;
	}
	static Distance distance(const point_type& a, const point_type& b) {
		Distance result = 0;
		for (int d = 0; d < Dimension; ++d) result += square_difference(a[d], b[d]);
		return result;
	}
	static Distance lower_bound(const Node& node, const point_type& query) {
		Distance result = 0;
		for (int d = 0; d < Dimension; ++d) {
			if (query[d] < node.lower[d]) result += square_difference(query[d], node.lower[d]);
			else if (node.upper[d] < query[d]) result += square_difference(query[d], node.upper[d]);
		}
		return result;
	}
	std::uint32_t construct(int begin, int end) {
		std::uint32_t id = std::uint32_t(nodes_.size());
		Node node{points_[begin].point, points_[begin].point, begin, end, points_[begin].index};
		for (int i = begin + 1; i < end; ++i) {
			node.minimum = std::min(node.minimum, points_[i].index);
			for (int d = 0; d < Dimension; ++d) {
				node.lower[d] = std::min(node.lower[d], points_[i].point[d]);
				node.upper[d] = std::max(node.upper[d], points_[i].point[d]);
			}
		}
		nodes_.push_back(node);
		if (end - begin <= leaf_size_) return id;
		int axis = 0;
		for (int d = 1; d < Dimension; ++d)
			if (Wide(node.upper[axis]) - Wide(node.lower[axis]) < Wide(node.upper[d]) - Wide(node.lower[d])) axis = d;
		int middle = begin + (end - begin) / 2;
		std::nth_element(points_.begin() + begin, points_.begin() + middle, points_.begin() + end,
			[axis](const Entry& a, const Entry& b) {
				return a.point[axis] < b.point[axis] || (a.point[axis] == b.point[axis] && a.index < b.index);
			});
		construct(begin, middle);
		nodes_[id].right = construct(middle, end);
		return id;
	}
	bool promising(Distance bound, std::uint32_t id, const neighbor_type& best) const {
		return Less{}({nodes_[id].minimum, bound}, best);
	}
	void nearest_node(std::uint32_t id, const point_type& query, int exclude, neighbor_type& best) const {
		const auto& node = nodes_[id];
		if (!node.right) {
			for (int i = node.begin; i < node.end; ++i) {
				const auto& point = points_[i];
				if (point.index == exclude) continue;
				neighbor_type candidate{point.index, distance(query, point.point)};
				if (Less{}(candidate, best)) best = candidate;
			}
			return;
		}
		std::uint32_t first = id + 1, second = node.right;
		Distance a = lower_bound(nodes_[first], query), b = lower_bound(nodes_[second], query);
		if (Less{}({nodes_[second].minimum, b}, {nodes_[first].minimum, a})) std::swap(first, second), std::swap(a, b);
		if (promising(a, first, best)) nearest_node(first, query, exclude, best);
		if (promising(b, second, best)) nearest_node(second, query, exclude, best);
	}
	void nearest_k_node(std::uint32_t id, const point_type& query, int k, int exclude, Heap& heap) const {
		const auto& node = nodes_[id];
		if (!node.right) {
			for (int i = node.begin; i < node.end; ++i) {
				const auto& point = points_[i];
				if (point.index == exclude) continue;
				neighbor_type candidate{point.index, distance(query, point.point)};
				if (int(heap.size()) < k) heap.push(candidate);
				else if (Less{}(candidate, heap.top())) heap.replace_top(candidate);
			}
			return;
		}
		std::uint32_t first = id + 1, second = node.right;
		Distance a = lower_bound(nodes_[first], query), b = lower_bound(nodes_[second], query);
		if (Less{}({nodes_[second].minimum, b}, {nodes_[first].minimum, a})) std::swap(first, second), std::swap(a, b);
		if (int(heap.size()) < k || promising(a, first, heap.top())) nearest_k_node(first, query, k, exclude, heap);
		if (int(heap.size()) < k || promising(b, second, heap.top())) nearest_k_node(second, query, k, exclude, heap);
	}
	static bool nonempty_box(const point_type& lower, const point_type& upper) {
		for (int d = 0; d < Dimension; ++d) if (!(lower[d] < upper[d])) return false;
		return true;
	}
	template<bool Report, class F>
	int range_node(std::uint32_t id, const point_type& lower, const point_type& upper, F& visit) const {
		const auto& node = nodes_[id];
		bool inside = true;
		for (int d = 0; d < Dimension; ++d) {
			if (node.upper[d] < lower[d] || !(node.lower[d] < upper[d])) return 0;
			inside &= !(node.lower[d] < lower[d]) && node.upper[d] < upper[d];
		}
		if (inside) {
			if constexpr (Report) for (int i = node.begin; i < node.end; ++i) visit(points_[i].index);
			return node.end - node.begin;
		}
		if (node.right) return range_node<Report>(id + 1, lower, upper, visit) + range_node<Report>(node.right, lower, upper, visit);
		int count = 0;
		for (int i = node.begin; i < node.end; ++i) {
			bool found = true;
			for (int d = 0; d < Dimension; ++d)
				found &= !(points_[i].point[d] < lower[d]) && points_[i].point[d] < upper[d];
			if (found) {
				if constexpr (Report) visit(points_[i].index);
				++count;
			}
		}
		return count;
	}

  public:
	KDTree() = default;
	explicit KDTree(std::span<const point_type> points, int leaf_size = 16) { build(points, leaf_size); }
	void build(std::span<const point_type> points, int leaf_size = 16) {
		assert(points.size() <= std::size_t(std::numeric_limits<int>::max()) && leaf_size > 0);
		KDTree next;
		next.leaf_size_ = leaf_size;
		next.points_.reserve(points.size());
		for (int i = 0; i < int(points.size()); ++i) {
			check_point(points[i]);
			next.points_.push_back({points[i], i});
		}
		next.positions_.resize(points.size());
		if (!points.empty()) {
			next.nodes_.reserve(std::min(points.size() * 2, (points.size() / std::size_t(leaf_size) + 1) * 4));
			next.construct(0, int(points.size()));
			for (int i = 0; i < int(points.size()); ++i) next.positions_[next.points_[i].index] = i;
		}
		*this = std::move(next);
	}
	int size() const { return int(points_.size()); }
	bool empty() const { return points_.empty(); }
	int leaf_size() const { return leaf_size_; }
	const point_type& point(int index) const {
		assert(0 <= index && index < size());
		return points_[positions_[index]].point;
	}
	std::optional<neighbor_type> nearest(const point_type& query, int exclude = -1) const {
		check_point(query);
		check_exclude(exclude);
		if (empty() || (size() == 1 && exclude == 0)) return std::nullopt;
		neighbor_type best{std::numeric_limits<int>::max(), std::numeric_limits<Distance>::max()};
		nearest_node(0, query, exclude, best);
		return best;
	}
	std::vector<neighbor_type> k_nearest(const point_type& query, int k, int exclude = -1) const {
		assert(k >= 0);
		check_point(query);
		check_exclude(exclude);
		k = std::min(k, size() - (exclude != -1));
		if (!k) return {};
		if (k == 1) return {*nearest(query, exclude)};
		Heap heap;
		heap.reserve(k);
		nearest_k_node(0, query, k, exclude, heap);
		auto result = heap.values();
		std::sort(result.begin(), result.end(), Less{});
		return result;
	}
	int range_count(const point_type& lower, const point_type& upper) const {
		if (empty() || !nonempty_box(lower, upper)) return 0;
		auto visit = [](int) {};
		return range_node<false>(0, lower, upper, visit);
	}
	template<class F> void range_visit(const point_type& lower, const point_type& upper, F&& visit) const {
		if (!empty() && nonempty_box(lower, upper)) range_node<true>(0, lower, upper, visit);
	}
	std::vector<int> range_report(const point_type& lower, const point_type& upper) const {
		std::vector<int> result;
		range_visit(lower, upper, [&](int index) { result.push_back(index); });
		return result;
	}
};


#line 1 "components/fast_io.hpp"



#line 6 "components/fast_io.hpp"
#include <bit>
#line 8 "components/fast_io.hpp"
#include <charconv>
#line 11 "components/fast_io.hpp"
#include <cstdio>
#include <cstring>
#include <memory>
#include <stdexcept>
#include <string>
#include <string_view>
#line 18 "components/fast_io.hpp"
#if defined(__unix__) || defined(__APPLE__)
#include <sys/mman.h>
#include <sys/stat.h>
#include <unistd.h>
#endif

namespace fast_io_detail { struct BulkInput; struct BulkOutput; }

class FastInput {
	friend struct fast_io_detail::BulkInput;
	static constexpr std::size_t capacity = 1 << 16;
	FILE *file = nullptr;
	std::unique_ptr<char[]> buffer;
	const char *current = "", *end = current;
	std::string scratch;
	void *mapping = nullptr;
	std::size_t mapped_size = 0;
	bool refill() {
		if (!file) return false;
		std::size_t n = std::fread(buffer.get(), 1, capacity, file);
		current = buffer.get();
		end = current + n;
		return n;
	}
	static const char *scan(const char *p, const char *end) {
		while (end - p >= 8) {
			std::uint64_t x;
			std::memcpy(&x, p, 8);
			if ((x - 0x2121212121212121ULL) & ~x & 0x8080808080808080ULL) break;
			p += 8;
		}
		while (p != end && static_cast<unsigned char>(*p) > ' ') ++p;
		return p;
	}

public:
	explicit FastInput(FILE *input = stdin) {
#if defined(__unix__) || defined(__APPLE__)
		struct stat status;
		int fd = fileno(input);
		auto offset = ftello(input);
		if (fd >= 0 && offset >= 0 && !fstat(fd, &status) && S_ISREG(status.st_mode) && status.st_size > offset) {
			mapped_size = status.st_size;
			mapping = mmap(nullptr, mapped_size, PROT_READ, MAP_PRIVATE, fd, 0);
			if (mapping != MAP_FAILED) {
				current = static_cast<const char *>(mapping) + offset;
				end = static_cast<const char *>(mapping) + mapped_size;
				return;
			}
			mapping = nullptr;
		}
#endif
		file = input;
		buffer = std::make_unique_for_overwrite<char[]>(capacity);
	}
	explicit FastInput(std::string_view input) : current(input.empty() ? "" : input.data()), end(current + input.size()) {}
	FastInput(const FastInput &) = delete;
	FastInput &operator=(const FastInput &) = delete;
	~FastInput() {
#if defined(__unix__) || defined(__APPLE__)
		if (mapping) munmap(mapping, mapped_size);
#endif
	}
	std::string_view token() {
		while (true) {
			while (current != end && static_cast<unsigned char>(*current) <= ' ') ++current;
			if (current != end) break;
			if (!refill()) return {};
		}
		const char *begin = current;
		current = scan(current, end);
		if (current != end || !file) return {begin, std::size_t(current - begin)};
		scratch.assign(begin, end);
		while (refill()) {
			current = scan(current, end);
			scratch.append(buffer.get(), std::size_t(current - buffer.get()));
			if (current != end) break;
		}
		return scratch;
	}
	template <class T>
	bool read(T &value) {
		if constexpr (std::is_integral_v<T> && !std::is_same_v<T, bool>) {
			while (true) {
				while (current != end && static_cast<unsigned char>(*current) <= ' ') ++current;
				if (current != end) break;
				if (!refill()) return false;
			}
			if constexpr (std::is_unsigned_v<T> && sizeof(T) == 4 && std::endian::native == std::endian::little) {
				if (end - current >= 11) {
					std::uint64_t x;
					std::memcpy(&x, current, 8);
					if (!(((x + 0x4646464646464646ULL) | (x - 0x3030303030303030ULL)) & 0x8080808080808080ULL)) {
						x &= 0x0f0f0f0f0f0f0f0fULL;
						x = (x * 10 + (x >> 8)) & 0x00ff00ff00ff00ffULL;
						x = (x * 100 + (x >> 16)) & 0x0000ffff0000ffffULL;
						std::uint64_t v = std::uint32_t(x * 10000 + (x >> 32));
						int n = 8;
						for (; n < 10 && static_cast<unsigned char>(current[n]) - unsigned('0') < 10; ++n)
							v = v * 10 + (current[n] - '0');
						if (static_cast<unsigned char>(current[n]) <= ' ' && v <= 0xffffffffULL) {
							current += n;
							value = T(v);
							return true;
						}
					}
				}
			}
			T parsed{};
			auto result = std::from_chars(current, end, parsed);
			if (result.ec == std::errc{} && (result.ptr != end ? static_cast<unsigned char>(*result.ptr) <= ' ' : !file)) {
				current = result.ptr;
				value = parsed;
				return true;
			}
		}
		auto s = token();
		if (s.empty()) return false;
		if constexpr (std::is_same_v<T, bool>) {
			assert(s == "0" || s == "1");
			value = s[0] == '1';
		} else if constexpr (std::is_integral_v<T>) {
			auto result = std::from_chars(s.data(), s.data() + s.size(), value);
			assert(result.ec == std::errc{} && result.ptr == s.data() + s.size());
			(void)result;
		} else value = T(s);
		return true;
	}
};

class FastOutput {
	friend struct fast_io_detail::BulkOutput;
	inline static constexpr auto quads = [] {
		std::array<std::array<char, 4>, 10000> table{};
		for (int i = 0; i < 10000; ++i)
			table[i] = {char('0' + i / 1000), char('0' + i / 100 % 10), char('0' + i / 10 % 10), char('0' + i % 10)};
		return table;
	}();
	FILE *file;
	std::unique_ptr<char[]> buffer = std::make_unique_for_overwrite<char[]>(1 << 16);
	std::size_t capacity = 1 << 16, length = 0;
	char *reserve(std::size_t n) {
		if (n > capacity - length) {
			if (!flush()) throw std::runtime_error("output failed");
			if (n > capacity) {
				std::size_t next = std::max(n, 2 * capacity);
				buffer = std::make_unique_for_overwrite<char[]>(next);
				capacity = next;
			}
		}
		return buffer.get() + length;
	}

public:
	explicit FastOutput(FILE *output = stdout) : file(output) {}
	FastOutput(const FastOutput &) = delete;
	FastOutput &operator=(const FastOutput &) = delete;
	~FastOutput() { flush(); }
	bool flush() {
		std::size_t offset = 0;
		while (offset < length) {
			auto n = std::fwrite(buffer.get() + offset, 1, length - offset, file);
			if (!n) {
				std::memmove(buffer.get(), buffer.get() + offset, length - offset);
				length -= offset;
				return false;
			}
			offset += n;
		}
		length = 0;
		return std::fflush(file) == 0;
	}
	void write(char c) { *reserve(1) = c; ++length; }
	void write(__uint128_t value) {
		char digits[40];
		char *end = digits + sizeof(digits), *begin = end;
		do { *--begin = char('0' + value % 10); value /= 10; } while (value);
		write(std::string_view(begin, end - begin));
	}
	void write(__int128_t value) {
		__uint128_t magnitude = __uint128_t(value);
		if (value < 0) write('-'), magnitude = 0 - magnitude;
		write(magnitude);
	}
	void write(std::string_view s) {
		char *out = reserve(s.size());
		if (!s.empty()) std::memcpy(out, s.data(), s.size());
		length += s.size();
	}
	template <class T> requires std::is_integral_v<T>
	void write(T value) {
		char *out = reserve(32);
		if constexpr (std::is_same_v<T, bool>) *out = '0' + value, ++length;
		else if constexpr (std::is_unsigned_v<T> && sizeof(T) == 4) {
			char *p = out;
			if (value >= 100000000) {
				auto high = value / 100000000, low = value % 100000000;
				p = std::to_chars(p, p + 2, high).ptr;
				std::memcpy(p, quads[low / 10000].data(), 4);
				std::memcpy(p + 4, quads[low % 10000].data(), 4);
				p += 8;
			} else if (value >= 10000) {
				p = std::to_chars(p, p + 4, value / 10000).ptr;
				std::memcpy(p, quads[value % 10000].data(), 4);
				p += 4;
			} else p = std::to_chars(p, p + 4, value).ptr;
			length += p - out;
		} else length += std::to_chars(out, out + 32, value).ptr - out;
	}
	template <class T> requires requires(const T &value, char *out) { value.decimal_capacity(); value.to_chars(out); }
	void write(const T &value) {
		char *out = reserve(value.decimal_capacity());
		length += value.to_chars(out) - out;
	}
	template <class T>
	void println(const T &value) { write(value); write('\n'); }
};


#line 7 "test/kd_tree.test.cpp"

int main() {
	FastInput input;
	FastOutput output;
	int cases = 0;
	input.read(cases);
	while (cases--) {
		int n = 0;
		input.read(n);
		using Tree = KDTree<std::int64_t, 2, std::int64_t>;
		std::vector<Tree::point_type> points(n);
		for (auto& point : points) input.read(point[0]), input.read(point[1]);
		Tree tree(points);
		std::int64_t distance = std::numeric_limits<std::int64_t>::max();
		int a = -1, b = -1;
		for (int i = 0; i < n; ++i) {
			auto nearest = tree.nearest(points[i], i);
			if (nearest->distance_squared < distance) {
				distance = nearest->distance_squared;
				a = i;
				b = nearest->index;
			}
		}
		output.write(a);
		output.write(' ');
		output.println(b);
	}
}