Merge 743199f6a4b88f3355e66eb84ac57152dde5a374 into fe6da3a16bd2d284085d35e450f2896b460b252b

irr/include/vector3d.h

@@ -7,6 +7,7 @@
 #include "irrMath.h"
 
 #include <functional>
+#include <array>
 
 namespace irr
 {
@@ -32,6 +33,9 @@ class vector3d
 	//! 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,26 @@ class vector3d
 		return *this;
 	}
 
+	std::array<T, 3> toArray() const {
+		return {X, Y, Z};
+	}
+
+	vector3d<T> min(const T min_component) const {
+		return vector3d<T>(
+			std::min(X, min_component),
+			std::min(Y, min_component),
+			std::min(Z, min_component)
+		);
+	}
+
+	vector3d<T> max(const T max_component) const {
+		return vector3d<T>(
+			std::max(X, max_component),
+			std::max(Y, max_component),
+			std::max(Z, max_component)
+		);
+	}
+
 	//! Get length of the vector.
 	T getLength() const { return core::squareroot(X * X + Y * Y + Z * Z); }
 

src/activeobjectmgr.h

@@ -55,7 +55,7 @@ class ActiveObjectMgr
 			for (auto &it : m_active_objects.iter()) {
 				if (!it.second)
 					continue;
-				m_active_objects.remove(it.first);
+				removeObject(it.first);
 			}
 		} while (!m_active_objects.empty());
 	}

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

src/collision.cpp

@@ -19,6 +19,7 @@ with this program; if not, write to the Free Software Foundation, Inc.,
 
 #include "collision.h"
 #include <cmath>
+#include "irr_aabb3d.h"
 #include "mapblock.h"
 #include "map.h"
 #include "nodedef.h"
@@ -281,13 +282,14 @@ static void add_object_boxes(Environment *env,
 		}
 	};
 
-	// Calculate distance by speed, add own extent and 1.5m of tolerance
-	const f32 distance = speed_f.getLength() * dtime +
-		box_0.getExtent().getLength() + 1.5f * BS;
+	const f32 tolerance = 1.5f * BS; // TODO increase tolerance
 
 #ifndef SERVER
 	ClientEnvironment *c_env = dynamic_cast<ClientEnvironment*>(env);
 	if (c_env) {
+		// Calculate distance by speed, add own extent and tolerance
+		const f32 distance = speed_f.getLength() * dtime +
+				box_0.getExtent().getLength() + tolerance;
 		std::vector<DistanceSortedActiveObject> clientobjects;
 		c_env->getActiveObjects(pos_f, distance, clientobjects);
 
@@ -326,9 +328,14 @@ static void add_object_boxes(Environment *env,
 				return false;
 			};
 
+			// Calculate distance by speed, add own extent and tolerance
+			const v3f movement = speed_f * dtime;
+			const v3f min = pos_f + box_0.MinEdge - v3f(tolerance) + movement.min(0);
+			const v3f max = pos_f + box_0.MaxEdge + v3f(tolerance) + movement.max(0);
+
 			// nothing is put into this vector
 			std::vector<ServerActiveObject*> s_objects;
-			s_env->getObjectsInsideRadius(s_objects, pos_f, distance, include_obj_cb);
+			s_env->getObjectsInArea(s_objects, aabb3f(min, max), include_obj_cb);
 		}
 	}
 }

src/server/activeobjectmgr.cpp

@@ -41,7 +41,7 @@ void ActiveObjectMgr::clearIf(const std::function<bool(ServerActiveObject *, u16
 			continue;
 		if (cb(it.second.get(), it.first)) {
 			// Remove reference from m_active_objects
-			m_active_objects.remove(it.first);
+			removeObject(it.first);
 		}
 	}
 }
@@ -83,16 +83,18 @@ 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);
+	assert(m_spatial_index.size() == m_active_objects.size());
 
 	auto new_size = m_active_objects.size();
 	verbosestream << "Server::ActiveObjectMgr::addActiveObjectRaw(): "
@@ -115,42 +117,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,

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 @@ class ActiveObjectMgr final : public ::ActiveObjectMgr<ServerActiveObject>
 	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 @@ class ActiveObjectMgr final : public ::ActiveObjectMgr<ServerActiveObject>
 	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

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;
 	}

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;
 }
 

src/server/player_sao.h

@@ -182,7 +182,7 @@ class PlayerSAO : public UnitSAO
 
 	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;
 

src/server/serveractiveobject.cpp

@@ -31,6 +31,13 @@ ServerActiveObject::ServerActiveObject(ServerEnvironment *env, v3f pos):
 {
 }
 
+void ServerActiveObject::setBasePosition(v3f pos) {
+	bool changed = m_base_position != pos;
+	m_base_position = pos;
+	if (changed && 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;

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 @@ class ServerActiveObject : public ActiveObject
 		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 @@ class ServerActiveObject : public ActiveObject
 	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 @@ class ServerActiveObject : public ActiveObject
 		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
 };

src/serverenvironment.cpp

@@ -21,6 +21,7 @@ with this program; if not, write to the Free Software Foundation, Inc.,
 #include <stack>
 #include <utility>
 #include "serverenvironment.h"
+#include "irr_aabb3d.h"
 #include "settings.h"
 #include "log.h"
 #include "mapblock.h"
@@ -1871,10 +1872,12 @@ void ServerEnvironment::getSelectedActiveObjects(
 		return false;
 	};
 
+	aabb3f search_area(shootline_on_map.start - 5 * BS, shootline_on_map.end + 5 * BS);
+	search_area.repair();
+
 	// Use "logic in callback" pattern to avoid useless vector filling
 	std::vector<ServerActiveObject*> tmp;
-	getObjectsInsideRadius(tmp, shootline_on_map.getMiddle(),
-		0.5 * shootline_on_map.getLength() + 5 * BS, process);
+	getObjectsInArea(tmp, search_area, process);
 }
 
 /*
@@ -2528,3 +2531,8 @@ bool ServerEnvironment::migrateAuthDatabase(
 	}
 	return true;
 }
+
+// HACK
+void ServerEnvironment_updatePos(ServerEnvironment *senv, const v3f &pos, u16 id) {
+	senv->updatePos(pos, id);
+}

src/serverenvironment.h

@@ -334,6 +334,10 @@ class ServerEnvironment final : public Environment
 	// 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 @@ class ServerEnvironment final : public Environment
 	std::unique_ptr<ServerActiveObject> createSAO(ActiveObjectType type, v3f pos,
 			const std::string &data);
 };
+
+// HACK
+void ServerEnvironment_updatePos(ServerEnvironment *senv, const v3f &pos, u16 id);

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

src/unittest/test_k_d_tree.cpp

@@ -0,0 +1,145 @@
+// 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);
+	}
+}
+
+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 testRandomQuery = [&]() {
+		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());
+	};
+
+	const auto testRandomQueries = [&]() {
+		for (int i = 0; i < 1000; ++i) {
+			testRandomQuery();
+		}
+	};
+
+	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();
+
+	// Finally, empty the structure(s)
+	for (u16 id = 800; id < 1000; ++id) {
+		objvec.remove(id);
+		kds.remove(id);
+		testRandomQuery();
+	}
+}

src/util/k_d_tree.h

@@ -0,0 +1,499 @@
+// 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>
+
+/*
+This implements a dynamic forest of static k-d-trees.
+
+A k-d-tree is a k-dimensional binary search tree.
+On the i-th level of the tree, you split by the (i mod k)-th coordinate.
+
+Building a balanced k-d-tree for n points is done in O(n log n) time:
+Points are stored in a matrix, identified by indices.
+These indices are presorted by all k axes.
+To split, you simply pick the pivot index in the appropriate index array,
+and mark all points left to it by index in a bitset.
+This lets you then split the indices sorted by the other axes,
+while preserving the sorted order.
+
+This however only gives us a static spatial index.
+To make it dynamic, we keep a "forest" of k-d-trees of sizes of successive powers of two.
+When we insert a new tree, we check whether there already is a k-d-tree of the same size.
+If this is the case, we merge with that tree, giving us a tree of twice the size,
+and so on, until we find a free size.
+
+This means our "forest" corresponds to a bit pattern,
+where a set bit means a non-empty tree.
+Inserting a point is equivalent to incrementing this bit pattern.
+
+To handle deletions, we simply mark the appropriate point as deleted using another bitset.
+When more than half the points have been deleted,
+we shrink the structure by removing all deleted points.
+This is equivalent to shifting down the "bit pattern" by one.
+
+There are plenty variations that could be explored:
+
+* Keeping a small amount of points in a small pool to make updates faster -
+  avoid building and querying small k-d-trees.
+  This might be useful if the overhead for small sizes hurts performance.
+* Keeping fewer trees to make queries faster, at the expense of updates.
+* More eagerly removing entries marked as deleted (for example, on merge).
+* Replacing the array-backed structure with a structure of dynamically allocated nodes.
+  This would make it possible to "let trees get out of shape".
+* Shrinking the structure currently sorts the live points by all axes,
+  not leveraging the existing presorting of the subsets.
+  Cleverly done filtering followed by sorted merges should enable linear time.
+*/
+
+using Idx = uint16_t; // TODO unify with Id
+// Use a separate, larger type for sizes than for indices
+// to make sure there are no wraparounds when we approach the limit.
+// This hardly affects performance or memory usage;
+// the core arrays still only store indices.
+using Size = uint32_t;
+
+// TODO more doc comments
+
+// 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) {}
+	//! Allocating constructor; leaves coords uninitialized!
+	Points(Size n) : n(n), coords(new Component[Dim * n]) {}
+	//! Copying constructor
+	Points(Size 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));
+	}
+	Size 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];
+	}
+	Component *begin(uint8_t d) {
+	 	return coords.get() + d * static_cast<std::size_t>(n);
+	}
+	Component *end(uint8_t d) {
+	 	return begin(d) + n;
+	}
+	const Component *begin(uint8_t d) const {
+	 	return coords.get() + d * static_cast<std::size_t>(n);
+	}
+	const Component *end(uint8_t d) const {
+	 	return begin(d) + n;
+	}
+private:
+	Size n;
+	std::unique_ptr<Component[]> coords;
+};
+
+template<uint8_t Dim>
+class SortedIndices {
+public:
+	//! empty
+	SortedIndices() : indices() {}
+
+	//! uninitialized indices
+	static SortedIndices newUninitialized(Size n) {
+		return SortedIndices(Points<Dim, Idx>(n));
+	}
+
+	//! Identity permutation on all axes
+	SortedIndices(Size n) : indices(n) {
+		for (uint8_t d = 0; d < Dim; ++d)
+			for (Idx i = 0; i < n; ++i)
+				indices.begin(d)[i] = i;
+	}
+
+	Size 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) {
+		return indices.begin(d);
+	}
+	Idx *end(uint8_t d) {
+		return indices.end(d);
+	}
+	const Idx *begin(uint8_t d) const {
+		return indices.begin(d);
+	}
+	const Idx *end(uint8_t d) const {
+		return indices.end(d);
+	}
+private:
+	SortedIndices(Points<Dim, Idx> &&indices) : indices(std::move(indices)) {}
+	Points<Dim, Idx> indices;
+};
+
+template<uint8_t Dim, class Component>
+class SortedPoints {
+public:
+	SortedPoints() : points(), indices() {}
+
+	//! Single point
+	SortedPoints(const std::array<Component, Dim> &point) : points(1), indices(1) {
+		points.setPoint(0, point);
+	}
+
+	//! Sort points
+	SortedPoints(Size 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++);
+		}
+	}
+
+	Size size() const {
+		// technically redundant with indices.size(),
+		// but that is irrelevant
+		return points.size();
+	}
+
+	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
+	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(Size 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());
+		// Note: Initialize `deleted` *before* calling `init`,
+		// since `init` abuses the `deleted` marks as left/right marks.
+		deleted = std::vector<bool>(cap());
+		init(0, 0, items.indices);
+		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 {
+		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
+	Size 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) {
+		// Temporarily 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>
+	// Note: root is of type std::size_t to avoid issues with wraparound
+	void rangeQuery(std::size_t root, uint8_t split,
+			const Point &min, const Point &max,
+			const F &cb) const {
+		if (root >= cap())
+			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(point, 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;
+};
+
+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, Id id) {
+		Tree tree(point, id);
+		for (uint8_t tree_idx = 0;; ++tree_idx) {
+			if (tree_idx == trees.size()) {
+				trees.push_back(std::move(tree));
+				updateDelEntries(tree_idx);
+				break;
+			}
+			if (trees[tree_idx].empty()) {
+				trees[tree_idx] = std::move(tree);
+				updateDelEntries(tree_idx);
+				break;
+			}
+			tree = Tree(tree, trees[tree_idx]);
+			trees[tree_idx] = std::move(Tree());
+		}
+		++n_entries;
+	}
+	void remove(Id id) {
+		const auto it = del_entries.find(id);
+		assert(it != del_entries.end());
+		trees.at(it->second.tree_idx).remove(it->second.in_tree);
+		del_entries.erase(it);
+		++deleted;
+		if (deleted >= (n_entries+1)/2) // "shift out" the last tree
+			shrink_to_half();
+	}
+	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);
+	}
+	Size size() const {
+		return n_entries - deleted;
+	}
+private:
+	void updateDelEntries(uint8_t tree_idx) {
+		trees[tree_idx].foreach([&](Idx in_tree_idx, auto _, Id id) {
+			del_entries[id] = {tree_idx, in_tree_idx};
+		});
+	}
+	// Shrink to half the size, equivalent to shifting down the "bit pattern".
+	void shrink_to_half() {
+		assert(n_entries >= deleted);
+		assert(n_entries - deleted == (n_entries >> 1));
+		n_entries -= deleted;
+		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);
+		Size i = 0;
+		for (const auto &tree : trees) {
+			tree.foreach([&](Idx _, auto point, Id id) {
+				assert(i < n_entries);
+				live_points.setPoint(static_cast<Idx>(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;
+		Size n = 1;
+		for (uint8_t d = 0; d < Dim; ++d)
+			point_ptrs[d] = live_points.begin(d);
+		for (uint8_t tree_idx = 0; tree_idx < trees.size() - 1; ++tree_idx, n *= 2) {
+			Tree tree;
+			if (!trees[tree_idx+1].empty()) {
+				tree = std::move(Tree(n, id_ptr, point_ptrs));
+				id_ptr += n;
+				for (uint8_t d = 0; d < Dim; ++d)
+					point_ptrs[d] += n;
+			}
+			trees[tree_idx] = std::move(tree);
+			updateDelEntries(tree_idx);
+		}
+		trees.pop_back(); // "shift out" tree with the most elements
+	}
+	// This could even use an array instead of a vector,
+	// since the number of trees is guaranteed to be logarithmic in the max of Idx
+	std::vector<Tree> trees;
+	struct DelEntry {
+		uint8_t tree_idx;
+		Idx in_tree;
+	};
+	std::unordered_map<Id, DelEntry> del_entries;
+	Size n_entries = 0;
+	Size deleted = 0;
+};