Merge 84bcdd98a7 into c4703a7f19

2024-05-17 20:09:01 +01:00 · 2024-05-17 20:09:01 +01:00 · bbb0479cdf
parent c4703a7f19 84bcdd98a7
commit bbb0479cdf
14 changed files with 672 additions and 49 deletions
--- a/irr/include/vector3d.h
+++ b/irr/include/vector3d.h
@ -7,6 +7,7 @@
 #include "irrMath.h"

 #include <functional>
+#include <array>

 namespace irr
 {
@ -32,6 +33,9 @@ public:
 	//! Constructor with the same value for all elements
 	explicit constexpr vector3d(T n) :
 			X(n), Y(n), Z(n) {}
+	//! Array - vector conversion
+	constexpr vector3d(const std::array<T, 3> &arr) :
+			X(arr[0]), Y(arr[1]), Z(arr[2]) {}

 	// operators

@ -181,6 +185,10 @@ public:
 		return *this;
 	}

+	std::array<T, 3> toArray() const {
+		return {X, Y, Z};
+	}
+
 	//! Get length of the vector.
 	T getLength() const { return core::squareroot(X * X + Y * Y + Z * Z); }

--- a/src/benchmark/benchmark_activeobjectmgr.cpp
+++ b/src/benchmark/benchmark_activeobjectmgr.cpp
@ -105,7 +105,11 @@ void benchGetObjectsInArea(Catch::Benchmark::Chronometer &meter)
 TEST_CASE("ActiveObjectMgr") {
 	BENCH_INSIDE_RADIUS(200)
 	BENCH_INSIDE_RADIUS(1450)
+	BENCH_INSIDE_RADIUS(10000)

 	BENCH_IN_AREA(200)
 	BENCH_IN_AREA(1450)
+	BENCH_IN_AREA(10000)
 }
+
+// TODO benchmark active object manager update costs
--- a/src/server/activeobjectmgr.cpp
+++ b/src/server/activeobjectmgr.cpp
@ -83,16 +83,17 @@ bool ActiveObjectMgr::registerObject(std::unique_ptr<ServerActiveObject> obj)
 		return false;
 	}

-	if (objectpos_over_limit(obj->getBasePosition())) {
-		v3f p = obj->getBasePosition();
+	const v3f pos = obj->getBasePosition();
+	if (objectpos_over_limit(pos)) {
 		warningstream << "Server::ActiveObjectMgr::addActiveObjectRaw(): "
-				<< "object position (" << p.X << "," << p.Y << "," << p.Z
+				<< "object position (" << pos.X << "," << pos.Y << "," << pos.Z
 				<< ") outside maximum range" << std::endl;
 		return false;
 	}

 	auto obj_id = obj->getId(); 
 	m_active_objects.put(obj_id, std::move(obj));
+	m_spatial_index.insert(pos.toArray(), obj_id);

 	auto new_size = m_active_objects.size();
 	verbosestream << "Server::ActiveObjectMgr::addActiveObjectRaw(): "
@ -115,42 +116,45 @@ void ActiveObjectMgr::removeObject(u16 id)
 	if (!ok) {
 		infostream << "Server::ActiveObjectMgr::removeObject(): "
 				<< "id=" << id << " not found" << std::endl;
+	} else {
+		m_spatial_index.remove(id);
 	}
 }

+void ActiveObjectMgr::updatePos(const v3f &pos, u16 id) {
+	// laziggy solution: only update if we already know the object
+	if (m_active_objects.get(id) != nullptr)
+		m_spatial_index.update(pos.toArray(), id);
+}
+
 void ActiveObjectMgr::getObjectsInsideRadius(const v3f &pos, float radius,
 		std::vector<ServerActiveObject *> &result,
 		std::function<bool(ServerActiveObject *obj)> include_obj_cb)
 {
-	float r2 = radius * radius;
-	for (auto &activeObject : m_active_objects.iter()) {
-		ServerActiveObject *obj = activeObject.second.get();
-		if (!obj)
-			continue;
-		const v3f &objectpos = obj->getBasePosition();
-		if (objectpos.getDistanceFromSQ(pos) > r2)
-			continue;
+	float r_squared = radius * radius;
+	m_spatial_index.rangeQuery((pos - v3f(radius)).toArray(), (pos + v3f(radius)).toArray(), [&](auto objPos, u16 id) {
+		if (v3f(objPos).getDistanceFromSQ(pos) > r_squared)
+			return;

+		auto obj = m_active_objects.get(id).get();
+		if (!obj)
+			return;
 		if (!include_obj_cb || include_obj_cb(obj))
 			result.push_back(obj);
-	}
+	});
 }

 void ActiveObjectMgr::getObjectsInArea(const aabb3f &box,
 		std::vector<ServerActiveObject *> &result,
 		std::function<bool(ServerActiveObject *obj)> include_obj_cb)
 {
-	for (auto &activeObject : m_active_objects.iter()) {
-		ServerActiveObject *obj = activeObject.second.get();
+	m_spatial_index.rangeQuery(box.MinEdge.toArray(), box.MaxEdge.toArray(), [&](auto _, u16 id) {
+		auto obj = m_active_objects.get(id).get();
 		if (!obj)
-			continue;
-		const v3f &objectpos = obj->getBasePosition();
-		if (!box.isPointInside(objectpos))
-			continue;
-
+			return;
 		if (!include_obj_cb || include_obj_cb(obj))
 			result.push_back(obj);
-	}
+	});
 }

 void ActiveObjectMgr::getAddedActiveObjectsAroundPos(v3f player_pos, f32 radius,
--- a/src/server/activeobjectmgr.h
+++ b/src/server/activeobjectmgr.h
@ -23,6 +23,7 @@ with this program; if not, write to the Free Software Foundation, Inc.,
 #include <vector>
 #include "../activeobjectmgr.h"
 #include "serveractiveobject.h"
+#include "util/k_d_tree.h"

 namespace server
 {
@ -38,6 +39,8 @@ public:
 	bool registerObject(std::unique_ptr<ServerActiveObject> obj) override;
 	void removeObject(u16 id) override;

+	void updatePos(const v3f &pos, u16 id);
+
 	void getObjectsInsideRadius(const v3f &pos, float radius,
 			std::vector<ServerActiveObject *> &result,
 			std::function<bool(ServerActiveObject *obj)> include_obj_cb);
@ -48,5 +51,7 @@ public:
 	void getAddedActiveObjectsAroundPos(v3f player_pos, f32 radius,
 			f32 player_radius, const std::set<u16> &current_objects,
 			std::vector<u16> &added_objects);
+private:
+	DynamicKdTrees<3, f32, u16> m_spatial_index;
 };
 } // namespace server
--- a/src/server/luaentity_sao.cpp
+++ b/src/server/luaentity_sao.cpp
@ -162,7 +162,7 @@ void LuaEntitySAO::step(float dtime, bool send_recommended)
 	// Each frame, parent position is copied if the object is attached, otherwise it's calculated normally
 	// If the object gets detached this comes into effect automatically from the last known origin
 	if (auto *parent = getParent()) {
-		m_base_position = parent->getBasePosition();
+		setBasePosition(parent->getBasePosition());
 		m_velocity = v3f(0,0,0);
 		m_acceleration = v3f(0,0,0);
 	} else {
@ -171,7 +171,7 @@ void LuaEntitySAO::step(float dtime, bool send_recommended)
 			box.MinEdge *= BS;
 			box.MaxEdge *= BS;
 			f32 pos_max_d = BS*0.25; // Distance per iteration
-			v3f p_pos = m_base_position;
+			v3f p_pos = getBasePosition();
 			v3f p_velocity = m_velocity;
 			v3f p_acceleration = m_acceleration;
 			moveresult = collisionMoveSimple(m_env, m_env->getGameDef(),
@ -181,11 +181,11 @@ void LuaEntitySAO::step(float dtime, bool send_recommended)
 			moveresult_p = &moveresult;

 			// Apply results
-			m_base_position = p_pos;
+			setBasePosition(p_pos);
 			m_velocity = p_velocity;
 			m_acceleration = p_acceleration;
 		} else {
-			m_base_position += (m_velocity + m_acceleration * 0.5f * dtime) * dtime;
+			addPos((m_velocity + m_acceleration * 0.5f * dtime) * dtime);
 			m_velocity += dtime * m_acceleration;
 		}

@ -228,7 +228,7 @@ void LuaEntitySAO::step(float dtime, bool send_recommended)
 		} else if(m_last_sent_position_timer > 0.2){
 			minchange = 0.05*BS;
 		}
-		float move_d = m_base_position.getDistanceFrom(m_last_sent_position);
+		float move_d = getBasePosition().getDistanceFrom(m_last_sent_position);
 		move_d += m_last_sent_move_precision;
 		float vel_d = m_velocity.getDistanceFrom(m_last_sent_velocity);
 		if (move_d > minchange || vel_d > minchange ||
@ -252,7 +252,7 @@ std::string LuaEntitySAO::getClientInitializationData(u16 protocol_version)
 	os << serializeString16(m_init_name); // name
 	writeU8(os, 0); // is_player
 	writeU16(os, getId()); //id
-	writeV3F32(os, m_base_position);
+	writeV3F32(os, getBasePosition());
 	writeV3F32(os, m_rotation);
 	writeU16(os, m_hp);

@ -381,7 +381,7 @@ void LuaEntitySAO::setPos(const v3f &pos)
 {
 	if(isAttached())
 		return;
-	m_base_position = pos;
+	setBasePosition(pos);
 	sendPosition(false, true);
 }

@ -389,7 +389,7 @@ void LuaEntitySAO::moveTo(v3f pos, bool continuous)
 {
 	if(isAttached())
 		return;
-	m_base_position = pos;
+	setBasePosition(pos);
 	if(!continuous)
 		sendPosition(true, true);
 }
@ -403,7 +403,7 @@ std::string LuaEntitySAO::getDescription()
 {
 	std::ostringstream oss;
 	oss << "LuaEntitySAO \"" << m_init_name << "\" ";
-	auto pos = floatToInt(m_base_position, BS);
+	auto pos = floatToInt(getBasePosition(), BS);
 	oss << "at " << pos;
 	return oss.str();
 }
@ -521,10 +521,10 @@ void LuaEntitySAO::sendPosition(bool do_interpolate, bool is_movement_end)
 	// Send attachment updates instantly to the client prior updating position
 	sendOutdatedData();

-	m_last_sent_move_precision = m_base_position.getDistanceFrom(
+	m_last_sent_move_precision = getBasePosition().getDistanceFrom(
 			m_last_sent_position);
 	m_last_sent_position_timer = 0;
-	m_last_sent_position = m_base_position;
+	m_last_sent_position = getBasePosition();
 	m_last_sent_velocity = m_velocity;
 	//m_last_sent_acceleration = m_acceleration;
 	m_last_sent_rotation = m_rotation;
@ -532,7 +532,7 @@ void LuaEntitySAO::sendPosition(bool do_interpolate, bool is_movement_end)
 	float update_interval = m_env->getSendRecommendedInterval();

 	std::string str = generateUpdatePositionCommand(
-		m_base_position,
+		getBasePosition(),
 		m_velocity,
 		m_acceleration,
 		m_rotation,
@ -552,8 +552,8 @@ bool LuaEntitySAO::getCollisionBox(aabb3f *toset) const
 		toset->MinEdge = m_prop.collisionbox.MinEdge * BS;
 		toset->MaxEdge = m_prop.collisionbox.MaxEdge * BS;

-		toset->MinEdge += m_base_position;
-		toset->MaxEdge += m_base_position;
+		toset->MinEdge += getBasePosition();
+		toset->MaxEdge += getBasePosition();

 		return true;
 	}
--- a/src/server/player_sao.cpp
+++ b/src/server/player_sao.cpp
@ -86,11 +86,10 @@ std::string PlayerSAO::getDescription()
 void PlayerSAO::addedToEnvironment(u32 dtime_s)
 {
 	ServerActiveObject::addedToEnvironment(dtime_s);
-	ServerActiveObject::setBasePosition(m_base_position);
 	m_player->setPlayerSAO(this);
 	m_player->setPeerId(m_peer_id_initial);
 	m_peer_id_initial = PEER_ID_INEXISTENT; // don't try to use it again.
-	m_last_good_position = m_base_position;
+	m_last_good_position = getBasePosition();
 }

 // Called before removing from environment
@ -116,7 +115,7 @@ std::string PlayerSAO::getClientInitializationData(u16 protocol_version)
 	os << serializeString16(m_player->getName()); // name
 	writeU8(os, 1); // is_player
 	writeS16(os, getId()); // id
-	writeV3F32(os, m_base_position);
+	writeV3F32(os, getBasePosition());
 	writeV3F32(os, m_rotation);
 	writeU16(os, getHP());

@ -195,7 +194,7 @@ void PlayerSAO::step(float dtime, bool send_recommended)
 		// Sequence of damage points, starting 0.1 above feet and progressing
 		// upwards in 1 node intervals, stopping below top damage point.
 		for (float dam_height = 0.1f; dam_height < dam_top; dam_height++) {
-			v3s16 p = floatToInt(m_base_position +
+			v3s16 p = floatToInt(getBasePosition() +
 				v3f(0.0f, dam_height * BS, 0.0f), BS);
 			MapNode n = m_env->getMap().getNode(p);
 			const ContentFeatures &c = m_env->getGameDef()->ndef()->get(n);
@ -207,7 +206,7 @@ void PlayerSAO::step(float dtime, bool send_recommended)
 		}

 		// Top damage point
-		v3s16 ptop = floatToInt(m_base_position +
+		v3s16 ptop = floatToInt(getBasePosition() +
 			v3f(0.0f, dam_top * BS, 0.0f), BS);
 		MapNode ntop = m_env->getMap().getNode(ptop);
 		const ContentFeatures &c = m_env->getGameDef()->ndef()->get(ntop);
@ -285,7 +284,7 @@ void PlayerSAO::step(float dtime, bool send_recommended)
 		if (isAttached())
 			pos = m_last_good_position;
 		else
-			pos = m_base_position;
+			pos = getBasePosition();

 		std::string str = generateUpdatePositionCommand(
 			pos,
@ -344,7 +343,7 @@ std::string PlayerSAO::generateUpdatePhysicsOverrideCommand() const

 void PlayerSAO::setBasePosition(v3f position)
 {
-	if (m_player && position != m_base_position)
+	if (m_player && position != getBasePosition())
 		m_player->setDirty(true);

 	// This needs to be ran for attachments too
@ -629,7 +628,7 @@ bool PlayerSAO::checkMovementCheat()
 	if (m_is_singleplayer ||
 			isAttached() ||
 			g_settings->getBool("disable_anticheat")) {
-		m_last_good_position = m_base_position;
+		m_last_good_position = getBasePosition();
 		return false;
 	}

@ -694,7 +693,7 @@ bool PlayerSAO::checkMovementCheat()
 	if (player_max_jump < 0.0001f)
 		player_max_jump = 0.0001f;

-	v3f diff = (m_base_position - m_last_good_position);
+	v3f diff = (getBasePosition() - m_last_good_position);
 	float d_vert = diff.Y;
 	diff.Y = 0;
 	float d_horiz = diff.getLength();
@ -710,7 +709,7 @@ bool PlayerSAO::checkMovementCheat()
 	}

 	if (m_move_pool.grab(required_time)) {
-		m_last_good_position = m_base_position;
+		m_last_good_position = getBasePosition();
 	} else {
 		const float LAG_POOL_MIN = 5.0;
 		float lag_pool_max = m_env->getMaxLagEstimate() * 2.0;
@ -732,8 +731,8 @@ bool PlayerSAO::getCollisionBox(aabb3f *toset) const
 	toset->MinEdge = m_prop.collisionbox.MinEdge * BS;
 	toset->MaxEdge = m_prop.collisionbox.MaxEdge * BS;

-	toset->MinEdge += m_base_position;
-	toset->MaxEdge += m_base_position;
+	toset->MinEdge += getBasePosition();
+	toset->MaxEdge += getBasePosition();
 	return true;
 }

--- a/src/server/player_sao.h
+++ b/src/server/player_sao.h
@ -182,7 +182,7 @@ public:

 	void finalize(RemotePlayer *player, const std::set<std::string> &privs);

-	v3f getEyePosition() const { return m_base_position + getEyeOffset(); }
+	v3f getEyePosition() const { return getBasePosition() + getEyeOffset(); }
 	v3f getEyeOffset() const;
 	float getZoomFOV() const;

--- a/src/server/serveractiveobject.cpp
+++ b/src/server/serveractiveobject.cpp
@ -31,6 +31,12 @@ ServerActiveObject::ServerActiveObject(ServerEnvironment *env, v3f pos):
 {
 }

+void ServerActiveObject::setBasePosition(v3f pos) {
+	m_base_position = pos;
+	if (m_env) // HACK this doesn't feel right; *when* is m_env null?
+		ServerEnvironment_updatePos(m_env, pos, getId());
+}
+
 float ServerActiveObject::getMinimumSavedMovement()
 {
 	return 2.0*BS;
--- a/src/server/serveractiveobject.h
+++ b/src/server/serveractiveobject.h
@ -44,6 +44,7 @@ Some planning
 */

 class ServerEnvironment;
+void ServerEnvironment_updatePos(ServerEnvironment *senv, const v3f &pos, u16 id);
 struct ItemStack;
 struct ToolCapabilities;
 struct ObjectProperties;
@ -77,7 +78,7 @@ public:
 		Some simple getters/setters
 	*/
 	v3f getBasePosition() const { return m_base_position; }
-	void setBasePosition(v3f pos){ m_base_position = pos; }
+	void setBasePosition(v3f pos);
 	ServerEnvironment* getEnv(){ return m_env; }

 	/*
@ -244,7 +245,6 @@ protected:
 	virtual void onDetach(int parent_id) {}

 	ServerEnvironment *m_env;
-	v3f m_base_position;
 	std::unordered_set<u32> m_attached_particle_spawners;

 	/*
@ -272,4 +272,6 @@ protected:
 		Queue of messages to be sent to the client
 	*/
 	std::queue<ActiveObjectMessage> m_messages_out;
+private:
+	v3f m_base_position; // setBasePosition updates index and MUST be called
 };
--- a/src/serverenvironment.cpp
+++ b/src/serverenvironment.cpp
@ -2528,3 +2528,8 @@ bool ServerEnvironment::migrateAuthDatabase(
 	}
 	return true;
 }
+
+// HACK
+void ServerEnvironment_updatePos(ServerEnvironment *senv, const v3f &pos, u16 id) {
+	senv->updatePos(pos, id);
+}
--- a/src/serverenvironment.h
+++ b/src/serverenvironment.h
@ -334,6 +334,10 @@ public:
 	// Find the daylight value at pos with a Depth First Search
 	u8 findSunlight(v3s16 pos) const;

+	void updatePos(const v3f &pos, u16 id) {
+		return m_ao_manager.updatePos(pos, id);
+	}
+
 	// Find all active objects inside a radius around a point
 	void getObjectsInsideRadius(std::vector<ServerActiveObject *> &objects, const v3f &pos, float radius,
 			std::function<bool(ServerActiveObject *obj)> include_obj_cb)
@ -513,3 +517,6 @@ private:
 	std::unique_ptr<ServerActiveObject> createSAO(ActiveObjectType type, v3f pos,
 			const std::string &data);
 };
+
+// HACK
+void ServerEnvironment_updatePos(ServerEnvironment *senv, const v3f &pos, u16 id);
--- a/src/unittest/CMakeLists.txt
+++ b/src/unittest/CMakeLists.txt
@ -10,6 +10,7 @@ set (UNITTEST_SRCS
 	${CMAKE_CURRENT_SOURCE_DIR}/test_connection.cpp
 	${CMAKE_CURRENT_SOURCE_DIR}/test_craft.cpp
 	${CMAKE_CURRENT_SOURCE_DIR}/test_datastructures.cpp
+	${CMAKE_CURRENT_SOURCE_DIR}/test_k_d_tree.cpp
 	${CMAKE_CURRENT_SOURCE_DIR}/test_filesys.cpp
 	${CMAKE_CURRENT_SOURCE_DIR}/test_inventory.cpp
 	${CMAKE_CURRENT_SOURCE_DIR}/test_irrptr.cpp
--- a/src/unittest/test_k_d_tree.cpp
+++ b/src/unittest/test_k_d_tree.cpp
@ -0,0 +1,139 @@
+// Copyright (C) 2024 Lars Müller
+// SPDX-License-Identifier: LGPL-2.1-or-later
+
+#include "noise.h"
+#include "test.h"
+
+#include "util/k_d_tree.h"
+#include <algorithm>
+#include <unordered_set>
+
+class TestKdTree : public TestBase
+{
+public:
+	TestKdTree() { TestManager::registerTestModule(this); }
+	const char *getName() { return "TestKdTree"; }
+
+	void runTests(IGameDef *gamedef);
+
+	// TODO basic small cube test
+	void singleUpdate();
+	void randomOps();
+};
+
+template<uint8_t Dim, typename Component, typename Id>
+class ObjectVector {
+public:
+	using Point = std::array<Component, Dim>;
+	void insert(const Point &p, Id id) {
+		entries.push_back(Entry{p, id});
+	}
+	void remove(Id id) {
+		const auto it = std::find_if(entries.begin(), entries.end(), [&](const auto &e) {
+			return e.id == id;
+		});
+		assert(it != entries.end());
+		entries.erase(it);
+	}
+	void update(const Point &p, Id id) {
+		remove(id);
+		insert(p, id);
+	}
+	template<typename F>
+	void rangeQuery(const Point &min, const Point &max, const F &cb) {
+		for (const auto &e : entries) {
+			for (uint8_t d = 0; d < Dim; ++d)
+				if (e.point[d] < min[d] || e.point[d] > max[d])
+					goto next;
+			cb(e.point, e.id); // TODO check
+			next: {}
+		}
+	}
+private:
+	struct Entry {
+		Point point;
+		Id id;
+	};
+	std::vector<Entry> entries;
+};
+
+static TestKdTree g_test_instance;
+
+void TestKdTree::runTests(IGameDef *gamedef)
+{
+	rawstream << "-------- k-d-tree" << std::endl;
+	TEST(singleUpdate);
+	TEST(randomOps);
+}
+
+void TestKdTree::singleUpdate() {
+	DynamicKdTrees<3, u16, u16> kds;
+	for (u16 i = 1; i <= 5; ++i)
+		kds.insert({i, i, i}, i);
+	for (u16 i = 1; i <= 5; ++i) {
+		u16 j = i - 1;
+		kds.update({j, j, j}, i);
+	}
+}
+
+// 1: asan again
+// 2: asan again
+// 3: violates assert
+// 5: violates asan
+
+void TestKdTree::randomOps() {
+	PseudoRandom pr(814538);
+
+	ObjectVector<3, f32, u16> objvec;
+	DynamicKdTrees<3, f32, u16> kds;
+
+	const auto randPos = [&]() {
+		std::array<f32, 3> point;
+		for (uint8_t d = 0; d < 3; ++d)
+			point[d] = pr.range(-1000, 1000);
+		return point;
+	};
+
+	for (u16 id = 1; id < 1000; ++id) {
+		const auto point = randPos();
+		objvec.insert(point, id);
+		kds.insert(point, id);
+	}
+
+	const auto testRandomQueries = [&]() {
+		for (int i = 0; i < 1000; ++i) {
+			std::array<f32, 3> min, max;
+			for (uint8_t d = 0; d < 3; ++d) {
+				min[d] = pr.range(-1500, 1500);
+				max[d] = min[d] + pr.range(1, 2500);
+			}
+			std::unordered_set<u16> expected_ids;
+			objvec.rangeQuery(min, max, [&](auto _, u16 id) {
+				UASSERT(expected_ids.count(id) == 0);
+				expected_ids.insert(id);
+			});
+			kds.rangeQuery(min, max, [&](auto point, u16 id) {
+				UASSERT(expected_ids.count(id) == 1);
+				expected_ids.erase(id);
+			});
+			UASSERT(expected_ids.empty());
+		}
+	};
+
+	testRandomQueries();
+
+	for (u16 id = 1; id < 800; ++id) {
+		objvec.remove(id);
+		kds.remove(id);
+	}
+
+	testRandomQueries();
+
+	for (u16 id = 800; id < 1000; ++id) {
+		const auto point = randPos();
+		objvec.update(point, id);
+		kds.update(point, id);
+	}
+
+	testRandomQueries();
+}
--- a/src/util/k_d_tree.h
+++ b/src/util/k_d_tree.h
@ -0,0 +1,443 @@
+// Copyright (C) 2024 Lars Müller
+// SPDX-License-Identifier: LGPL-2.1-or-later
+
+#pragma once
+
+#include <algorithm>
+#include <cassert>
+#include <cstdint>
+#include <unordered_map>
+#include <vector>
+#include <memory>
+
+using Idx = uint16_t;
+
+// TODO docs and explanation
+
+// TODO profile and tweak knobs
+
+// TODO cleanup (split up in header and impl among other things)
+
+template<uint8_t Dim, typename Component>
+class Points {
+public:
+	using Point = std::array<Component, Dim>;
+	//! Empty
+	Points() : n(0), coords(nullptr) {}
+	//! Leaves coords uninitialized!
+	// TODO we want make_unique_for_overwrite here...
+	Points(Idx n) : n(n), coords(std::make_unique<Component[]>(Dim * n)) {}
+	//! Copying constructor
+	Points(Idx n, const std::array<Component const *, Dim> &coords) : Points(n) {
+		for (uint8_t d = 0; d < Dim; ++d)
+			std::copy(coords[d], coords[d] + n, begin(d));
+	}
+	Idx size() const {
+		return n;
+	}
+	void assign(Idx start, const Points &from) {
+		for (uint8_t d = 0; d < Dim; ++d)
+			std::copy(from.begin(d), from.end(d), begin(d) + start);
+	}
+	Point getPoint(Idx i) const {
+		Point point;
+		for (uint8_t d = 0; d < Dim; ++d)
+			point[d] = begin(d)[i];
+		return point;
+	}
+	void setPoint(Idx i, const Point &point) {
+		for (uint8_t d = 0; d < Dim; ++d)
+			begin(d)[i] = point[d];
+	}
+	// HACK interior mutability...
+	Component *begin(uint8_t d) const {
+	 	return coords.get() + d * n;
+	}
+	Component *end(uint8_t d) const {
+	 	return begin(d) + n;
+	}
+private:
+	Idx n;
+	std::unique_ptr<Component[]> coords;
+};
+
+template<uint8_t Dim>
+class SortedIndices {
+public:
+	//! empty
+	SortedIndices() : indices() {}
+
+	//! uninitialized indices
+	static SortedIndices newUninitialized(Idx n) {
+		return SortedIndices(n); // HACK can't be arsed to fix rn Points<Dim, Idx>(n));
+	}
+
+	// Identity permutation on all axes
+	SortedIndices(Idx n) : indices(n) {
+		for (uint8_t d = 0; d < Dim; ++d)
+			for (Idx i = 0; i < n; ++i)
+				indices.begin(d)[i] = i;
+	}
+
+	Idx size() const {
+		return indices.size();
+	}
+
+	bool empty() const {
+		return size() == 0;
+	}
+
+	struct SplitResult {
+		SortedIndices left, right;
+		Idx pivot;
+	};
+
+	//! Splits the sorted indices in the middle along the specified axis,
+	//! partitioning them into left (<=), the pivot, and right (>=).
+	SplitResult split(uint8_t axis, std::vector<bool> &markers) const {
+		const auto begin = indices.begin(axis);
+		Idx left_n = indices.size() / 2;
+		const auto mid = begin + left_n;
+
+		// Mark all points to be partitioned left
+		for (auto it = begin; it != mid; ++it)
+			markers[*it] = true;
+
+		SortedIndices left(left_n);
+		std::copy(begin, mid, left.indices.begin(axis));
+		SortedIndices right(indices.size() - left_n - 1);
+		std::copy(mid + 1, indices.end(axis), right.indices.begin(axis));
+
+		for (uint8_t d = 0; d < Dim; ++d) {
+			if (d == axis)
+				continue;
+			auto left_ptr = left.indices.begin(d);
+			auto right_ptr = right.indices.begin(d);
+			for (auto it = indices.begin(d); it != indices.end(d); ++it) {
+				if (*it != *mid) { // ignore pivot
+					if (markers[*it])
+						*(left_ptr++) = *it;
+					else
+						*(right_ptr++) = *it;
+				}
+			}
+		}
+
+		// Unmark points, since we want to reuse the storage for markers
+		for (auto it = begin; it != mid; ++it)
+			markers[*it] = false;
+
+		return SplitResult{std::move(left), std::move(right), *mid};
+	}
+
+	Idx *begin(uint8_t d) const {
+		return indices.begin(d);
+	}
+
+	Idx *end(uint8_t d) const {
+		return indices.end(d);
+	}
+private:
+	Points<Dim, Idx> indices;
+};
+
+template<uint8_t Dim, class Component>
+class SortedPoints {
+public:
+	SortedPoints() : points(), indices() {}
+
+	//! Single point
+	// TODO remove this
+	SortedPoints(const std::array<Component, Dim> &point) : points(1), indices(1) {
+		points.setPoint(0, point);
+	}
+
+	//! Sort points
+	SortedPoints(Idx n, const std::array<Component const *, Dim> ptrs)
+		: points(n, ptrs), indices(n)
+	{
+		for (uint8_t d = 0; d < Dim; ++d) {
+			const auto coord = points.begin(d);
+			std::sort(indices.begin(d), indices.end(d), [&](auto i, auto j) {
+				return coord[i] < coord[j];
+			});
+		}
+	}
+
+	//! Merge two sets of sorted points
+	SortedPoints(const SortedPoints &a, const SortedPoints &b)
+		: points(a.size() + b.size())
+	{
+		const auto n = points.size();
+		indices = SortedIndices<Dim>::newUninitialized(n);
+		for (uint8_t d = 0; d < Dim; ++d) {
+			points.assign(0, a.points);
+			points.assign(a.points.size(), b.points);
+			const auto coord = points.begin(d);
+			auto a_ptr = a.indices.begin(d);
+			auto b_ptr = b.indices.begin(d);
+			auto dst_ptr = indices.begin(d);
+			while (a_ptr != a.indices.end(d) && b_ptr != b.indices.end(d)) {
+				const auto i = *a_ptr;
+				const auto j = *b_ptr + a.size();
+				if (coord[i] <= coord[j]) {
+					*(dst_ptr++) = i;
+					++a_ptr;
+				} else {
+					*(dst_ptr++) = j;
+					++b_ptr;
+				}
+			}
+			while (a_ptr != a.indices.end(d))
+				*(dst_ptr++) = *(a_ptr++);
+			while (b_ptr != b.indices.end(d))
+				*(dst_ptr++) = a.size() + *(b_ptr++);
+		}
+	}
+
+	Idx size() const {
+		// technically redundant with indices.size(),
+		// but that is irrelevant
+		return points.size();
+	}
+	
+// HACK private:
+	Points<Dim, Component> points;
+	SortedIndices<Dim> indices;
+};
+
+template<uint8_t Dim, class Component, class Id>
+class KdTree {
+public:
+	using Point = std::array<Component, Dim>;
+
+	//! Empty tree
+	KdTree()
+		: items()
+		, ids(nullptr)
+		, tree(nullptr)
+		, deleted()
+	{}
+
+	//! Build a tree containing just a single point
+	// TODO this will probably be obsolete soon (TM)
+	KdTree(const Point &point, const Id &id)
+		: items(point)
+		, ids(std::make_unique<Id[]>(1))
+		, tree(std::make_unique<Idx[]>(1))
+		, deleted(1)
+	{
+		tree[0] = 0;
+		ids[0] = id;
+	}
+
+	//! Build a tree
+	KdTree(Idx n, Id const *ids, std::array<Component const *, Dim> pts)
+		: items(n, pts)
+		, ids(std::make_unique<Id[]>(n))
+		, tree(std::make_unique<Idx[]>(n))
+		, deleted(n)
+	{
+		std::copy(ids, ids + n, this->ids.get());
+		init(0, 0, items.indices);
+	}
+
+	//! Merge two trees. Both trees are assumed to have a power of two size.
+	KdTree(const KdTree &a, const KdTree &b)
+		: items(a.items, b.items)
+	{
+		tree = std::make_unique<Idx[]>(cap());
+		ids = std::make_unique<Id[]>(cap());
+		std::copy(a.ids.get(), a.ids.get() + a.cap(), ids.get());
+		std::copy(b.ids.get(), b.ids.get() + b.cap(), ids.get() + a.cap());
+		deleted = std::vector<bool>(cap());
+		init(0, 0, items.indices); // this does le dirty dirty hack so call it BEFORE we deal with deleted
+		std::copy(a.deleted.begin(), a.deleted.end(), deleted.begin());
+		std::copy(b.deleted.begin(), b.deleted.end(), deleted.begin() + a.items.size());
+	}
+
+	// TODO ray proximity query
+
+	template<typename F>
+	void rangeQuery(const Point &min, const Point &max,
+			const F &cb) const {
+		if (empty())
+			return;
+		rangeQuery(0, 0, min, max, cb);
+	}
+
+	void remove(Idx internalIdx) {
+		assert(!deleted[internalIdx]);
+		deleted[internalIdx] = true;
+	}
+
+	template<class F>
+	void foreach(F cb) const {
+		for (Idx i = 0; i < cap(); ++i)
+			if (!deleted[i])
+				cb(i, items.points.getPoint(i), ids[i]);
+	}
+
+	//! Capacity, not size, since some items may be marked as deleted
+	Idx cap() const {
+		return items.size();
+	}
+
+	//! "Empty" as in "never had anything"
+	bool empty() const {
+		return cap() == 0;
+	}
+
+private:
+	void init(Idx root, uint8_t axis, const SortedIndices<Dim> &sorted) {
+		// HACK abuse deleted marks as left/right marks
+		const auto split = sorted.split(axis, deleted);
+		tree[root] = split.pivot;
+		const auto next_axis = (axis + 1) % Dim;
+		if (!split.left.empty())
+			init(2 * root + 1, next_axis, split.left);
+		if (!split.right.empty())
+			init(2 * root + 2, next_axis, split.right);
+	}
+
+	template<typename F>
+	void rangeQuery(Idx root, uint8_t split,
+			const Point &min, const Point &max,
+			const F &cb) const {
+		if (root >= cap()) // TODO what if we overflow earlier?
+			return;
+		const auto ptid = tree[root];
+		const auto coord = items.points.begin(split)[ptid];
+		const auto leftChild = 2*root + 1;
+		const auto rightChild = 2*root + 2;
+		const auto nextSplit = (split + 1) % Dim;
+		if (min[split] > coord) {
+			rangeQuery(rightChild, nextSplit, min, max, cb);
+		} else if (max[split] < coord) {
+			rangeQuery(leftChild, nextSplit, min, max, cb);
+		} else {
+			rangeQuery(rightChild, nextSplit, min, max, cb);
+			rangeQuery(leftChild, nextSplit, min, max, cb);
+			if (deleted[ptid])
+				return;
+			const auto point = items.points.getPoint(ptid);
+			for (uint8_t d = 0; d < Dim; ++d)
+				if (point[d] < min[d] || point[d] > max[d])
+					return;
+			cb(items.points.getPoint(ptid), ids[ptid]);
+		}
+	}
+	SortedPoints<Dim, Component> items;
+	std::unique_ptr<Id[]> ids;
+	std::unique_ptr<Idx[]> tree;
+	// vector because this has the template specialization we want
+	// and i'm too lazy to implement bitsets myself right now
+	// just to shave off 16 redundant bytes (len + cap)
+	std::vector<bool> deleted;
+};
+
+// TODO abstract dynamic spatial index superclass
+template<uint8_t Dim, class Component, class Id>
+class DynamicKdTrees {
+	using Tree = KdTree<Dim, Component, Id>;
+public:
+	using Point = typename Tree::Point;
+	void insert(const std::array<Component, Dim> &point, const Id id) {
+		Tree tree(point, id);
+		for (uint8_t tree_idx = 0;; ++tree_idx) {
+			if (tree_idx >= trees.size()) {
+				tree.foreach([&](Idx in_tree_idx, auto _, Id id) {
+					del_entries[id] = {tree_idx, in_tree_idx};
+				});
+				trees.push_back(std::move(tree));
+				break;
+			}
+			if (trees[tree_idx].empty()) {
+				// TODO deduplicate
+				tree.foreach([&](Idx in_tree_idx, auto _, Id id) {
+					del_entries[id] = {tree_idx, in_tree_idx};
+				});
+				trees[tree_idx] = std::move(tree);
+				break;
+			}
+			tree = Tree(tree, trees[tree_idx]);
+			trees[tree_idx] = std::move(Tree());
+		}
+		++n_entries;
+	}
+	void remove(Id id) {
+		const auto del_entry = del_entries.at(id);
+		trees.at(del_entry.treeIdx).remove(del_entry.inTree);
+		del_entries.erase(id); // TODO use iterator right away...
+		++deleted;
+		if (deleted > n_entries/2) // we want to shift out the one!
+			compactify();
+	}
+	void update(const Point &newPos, Id id) {
+		remove(id);
+		insert(newPos, id);
+	}
+	template<typename F>
+	void rangeQuery(const Point &min, const Point &max,
+			const F &cb) const {
+		for (const auto &tree : trees)
+			tree.rangeQuery(min, max, cb);
+	}
+private:
+	void compactify() {
+		assert(n_entries >= deleted);
+		n_entries -= deleted; // note: this should be exactly n_entries/2
+		deleted = 0;
+		// reset map, freeing memory (instead of clearing)
+		del_entries = std::unordered_map<Id, DelEntry>();
+
+
+		// Collect all live points and corresponding IDs.
+		const auto live_ids = std::make_unique<Id[]>(n_entries);
+		Points<Dim, Component> live_points(n_entries);
+		Idx i = 0;
+		for (const auto &tree : trees) {
+			tree.foreach([&](Idx _, auto point, Id id) {
+				assert(i < n_entries);
+				live_points.setPoint(i, point);
+				live_ids[i] = id;
+				++i;
+			});
+		}
+		assert(i == n_entries);
+
+		// Construct a new forest.
+		// The "tree pattern" will effectively just be shifted down by one.
+		auto id_ptr = live_ids.get();
+		std::array<Component const *, Dim> point_ptrs;
+		Idx n = 1;
+		for (uint8_t d = 0; d < Dim; ++d)
+			point_ptrs[d] = live_points.begin(d);
+		for (uint8_t treeIdx = 0; treeIdx < trees.size() - 1; ++treeIdx, n *= 2) {
+			Tree tree;
+			if (!trees[treeIdx+1].empty()) {
+				// TODO maybe optimize from log² -> log?
+				// This could be achieved by doing a sorted merge of live points, then doing a radix sort.
+				tree = std::move(Tree(n, id_ptr, point_ptrs));
+				id_ptr += n;
+				for (uint8_t d = 0; d < Dim; ++d)
+					point_ptrs[d] += n;
+				// TODO dedupe
+				tree.foreach([&](Idx objIdx, auto _, Id id) {
+					del_entries[id] = {treeIdx, objIdx};
+				});
+			}
+			trees[treeIdx] = std::move(tree);
+		}
+		trees.pop_back(); // "shift out" tree with the most elements
+	}
+	// could use an array (rather than a vector) here since we've got a good bound on the size ahead of time but meh
+	std::vector<Tree> trees;
+	struct DelEntry {
+		uint8_t treeIdx;
+		Idx inTree;
+	};
+	std::unordered_map<Id, DelEntry> del_entries;
+	Idx n_entries = 0;
+	Idx deleted = 0;
+};