tour/minetest
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge 080be0c63b into f54d209bc8

Browse Source
This commit is contained in:
Lars Müller 2024-12-31 18:38:39 +00:00 committed by GitHub
commit 0a937a9f31
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
17 changed files with 1032 additions and 186 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
irr/include/vector3d.h
View File

@ -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,26 @@ public:
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); }

2
src/activeobjectmgr.h
View File

@ -40,7 +40,7 @@ public:
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());
}

4
src/benchmark/benchmark_activeobjectmgr.cpp
View File

@ -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

15
src/collision.cpp
View File

@ -4,6 +4,7 @@
#include "collision.h"
#include <cmath>
#include "irr_aabb3d.h"
#include "mapblock.h"
#include "map.h"
#include "nodedef.h"
@ -268,13 +269,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
#if CHECK_CLIENT_BUILD()
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);
@ -313,9 +315,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);
}
}
}

46
src/server/activeobjectmgr.cpp
View File

@ -26,7 +26,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);
}
}
}
@ -68,16 +68,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(): "
@ -100,6 +101,8 @@ void ActiveObjectMgr::removeObject(u16 id)
if (!ok) {
infostream << "Server::ActiveObjectMgr::removeObject(): "
<< "id=" << id << " not found" << std::endl;
} else {
m_spatial_index.remove(id);
}
}
@ -113,39 +116,40 @@ void ActiveObjectMgr::invalidateActiveObjectObserverCaches()
}
}
void ActiveObjectMgr::updatePos(u16 id, const v3f &pos) {
// HACK 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(

5
src/server/activeobjectmgr.h
View File

@ -8,6 +8,7 @@
#include <vector>
#include "../activeobjectmgr.h"
#include "serveractiveobject.h"
#include "util/k_d_tree.h"
namespace server
{
@ -25,6 +26,8 @@ public:
void invalidateActiveObjectObserverCaches();
void updatePos(u16 id, const v3f &pos);
void getObjectsInsideRadius(const v3f &pos, float radius,
std::vector<ServerActiveObject *> &result,
std::function<bool(ServerActiveObject *obj)> include_obj_cb);
@ -36,5 +39,7 @@ public:
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

28
src/server/luaentity_sao.cpp
View File

@ -147,7 +147,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 {
@ -155,7 +155,7 @@ void LuaEntitySAO::step(float dtime, bool send_recommended)
aabb3f box = m_prop.collisionbox;
box.MinEdge *= BS;
box.MaxEdge *= BS;
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(),
@ -165,11 +165,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;
}
@ -212,7 +212,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 ||
@ -236,7 +236,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);
@ -365,7 +365,7 @@ void LuaEntitySAO::setPos(const v3f &pos)
{
if(isAttached())
return;
m_base_position = pos;
setBasePosition(pos);
sendPosition(false, true);
}
@ -373,7 +373,7 @@ void LuaEntitySAO::moveTo(v3f pos, bool continuous)
{
if(isAttached())
return;
m_base_position = pos;
setBasePosition(pos);
if(!continuous)
sendPosition(true, true);
}
@ -387,7 +387,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();
}
@ -503,10 +503,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;
@ -514,7 +514,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,
@ -534,8 +534,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;
}

23
src/server/player_sao.cpp
View File

@ -71,11 +71,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
@ -101,7 +100,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());
@ -185,7 +184,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);
@ -197,7 +196,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);
@ -274,7 +273,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,
@ -333,7 +332,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
@ -637,7 +636,7 @@ bool PlayerSAO::checkMovementCheat()
if (m_is_singleplayer ||
isAttached() ||
!(anticheat_flags & AC_MOVEMENT)) {
m_last_good_position = m_base_position;
m_last_good_position = getBasePosition();
return false;
}
@ -702,7 +701,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();
@ -723,7 +722,7 @@ bool PlayerSAO::checkMovementCheat()
required_time /= anticheat_movement_tolerance;
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;
@ -745,8 +744,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;
}

2
src/server/player_sao.h
View File

@ -170,7 +170,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;

8
src/server/serveractiveobject.cpp
View File

@ -6,6 +6,7 @@
#include "inventory.h"
#include "inventorymanager.h"
#include "constants.h" // BS
#include "serverenvironment.h"
ServerActiveObject::ServerActiveObject(ServerEnvironment *env, v3f pos):
ActiveObject(0),
@ -14,6 +15,13 @@ ServerActiveObject::ServerActiveObject(ServerEnvironment *env, v3f pos):
{
}
void ServerActiveObject::setBasePosition(v3f pos) {
bool changed = m_base_position != pos;
m_base_position = pos;
if (changed && getEnv()) // HACK *when* is getEnv() null?
getEnv()->updatePos(getId(), pos);
}
float ServerActiveObject::getMinimumSavedMovement()
{
return 2.0*BS;

5
src/server/serveractiveobject.h
View File

@ -63,7 +63,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; }
/*
@ -245,7 +245,6 @@ protected:
virtual void onMarkedForRemoval() {}
ServerEnvironment *m_env;
v3f m_base_position;
std::unordered_set<u32> m_attached_particle_spawners;
/*
@ -273,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
};

7
src/serverenvironment.cpp
View File

@ -6,6 +6,7 @@
#include <stack>
#include <utility>
#include "serverenvironment.h"
#include "irr_aabb3d.h"
#include "settings.h"
#include "log.h"
#include "mapblock.h"
@ -1896,10 +1897,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);
}
/*

4
src/serverenvironment.h
View File

@ -334,6 +334,10 @@ public:
// Find the daylight value at pos with a Depth First Search
u8 findSunlight(v3s16 pos) const;
void updatePos(u16 id, const v3f &pos) {
return m_ao_manager.updatePos(id, pos);
}
// 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)

1
src/unittest/CMakeLists.txt
View File

@ -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

145
src/unittest/test_k_d_tree.cpp Normal file
View File

@ -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();
}
}

410
src/unittest/test_serveractiveobjectmgr.cpp
View File

@ -2,171 +2,323 @@
// SPDX-License-Identifier: LGPL-2.1-or-later
// Copyright (C) 2018 nerzhul, Loic Blot <loic.blot@unix-experience.fr>
#include "test.h"
#include "activeobjectmgr.h"
#include "catch.h"
#include "irrTypes.h"
#include "irr_aabb3d.h"
#include "mock_serveractiveobject.h"
#include <algorithm>
#include <queue>
#include <iterator>
#include <random>
#include <utility>
#include "server/activeobjectmgr.h"
#include "server/serveractiveobject.h"
#include "profiler.h"
class TestServerActiveObjectMgr {
server::ActiveObjectMgr saomgr;
std::vector<u16> ids;
class TestServerActiveObjectMgr : public TestBase
{
public:
TestServerActiveObjectMgr() { TestManager::registerTestModule(this); }
const char *getName() { return "TestServerActiveObjectMgr"; }
void runTests(IGameDef *gamedef);
u16 getFreeId() const {
return saomgr.getFreeId();
}
void testFreeID();
void testRegisterObject();
void testRemoveObject();
void testGetObjectsInsideRadius();
void testGetAddedActiveObjectsAroundPos();
bool registerObject(std::unique_ptr<ServerActiveObject> obj) {
auto *ptr = obj.get();
if (!saomgr.registerObject(std::move(obj)))
return false;
ids.push_back(ptr->getId());
return true;
}
void removeObject(u16 id) {
const auto it = std::find(ids.begin(), ids.end(), id);
REQUIRE(it != ids.end());
ids.erase(it);
saomgr.removeObject(id);
}
void updatePos(u16 id, const v3f &pos) {
auto *obj = saomgr.getActiveObject(id);
REQUIRE(obj != nullptr);
obj->setPos(pos);
saomgr.updatePos(id, pos); // HACK work around m_env == nullptr
}
void clear() {
saomgr.clear();
ids.clear();
}
ServerActiveObject *getActiveObject(u16 id) {
return saomgr.getActiveObject(id);
}
template<class T>
void getObjectsInsideRadius(T&& arg) {
saomgr.getObjectsInsideRadius(std::forward(arg));
}
template<class T>
void getAddedActiveObjectsAroundPos(T&& arg) {
saomgr.getAddedActiveObjectsAroundPos(std::forward(arg));
}
// Testing
bool empty() {
return ids.empty();
}
template<class T>
u16 randomId(T &random) {
REQUIRE(!ids.empty());
std::uniform_int_distribution<u16> index(0, ids.size() - 1);
return ids[index(random)];
}
void getObjectsInsideRadiusNaive(const v3f &pos, float radius,
std::vector<ServerActiveObject *> &result)
{
for (const auto &[id, obj] : saomgr.m_active_objects.iter()) {
if (obj->getBasePosition().getDistanceFromSQ(pos) <= radius * radius) {
result.push_back(obj.get());
}
}
}
void getObjectsInAreaNaive(const aabb3f &box,
std::vector<ServerActiveObject *> &result)
{
for (const auto &[id, obj] : saomgr.m_active_objects.iter()) {
if (box.isPointInside(obj->getBasePosition())) {
result.push_back(obj.get());
}
}
}
constexpr static auto compare_by_id = [](auto *sao1, auto *sao2) -> bool {
return sao1->getId() < sao2->getId();
};
static void sortById(std::vector<ServerActiveObject *> &saos) {
std::sort(saos.begin(), saos.end(), compare_by_id);
}
void compareObjects(std::vector<ServerActiveObject *> &actual,
std::vector<ServerActiveObject *> &expected)
{
std::vector<ServerActiveObject *> unexpected, missing;
sortById(actual);
sortById(expected);
std::set_difference(actual.begin(), actual.end(),
expected.begin(), expected.end(),
std::back_inserter(unexpected), compare_by_id);
assert(unexpected.empty());
std::set_difference(expected.begin(), expected.end(),
actual.begin(), actual.end(),
std::back_inserter(missing), compare_by_id);
assert(missing.empty());
}
void compareObjectsInsideRadius(const v3f &pos, float radius)
{
std::vector<ServerActiveObject *> actual, expected;
saomgr.getObjectsInsideRadius(pos, radius, actual, nullptr);
getObjectsInsideRadiusNaive(pos, radius, expected);
compareObjects(actual, expected);
}
void compareObjectsInArea(const aabb3f &box)
{
std::vector<ServerActiveObject *> actual, expected;
saomgr.getObjectsInArea(box, actual, nullptr);
getObjectsInAreaNaive(box, expected);
compareObjects(actual, expected);
}
};
static TestServerActiveObjectMgr g_test_instance;
void TestServerActiveObjectMgr::runTests(IGameDef *gamedef)
TEST_CASE("server active object manager")
{
TEST(testFreeID);
TEST(testRegisterObject)
TEST(testRemoveObject)
TEST(testGetObjectsInsideRadius);
TEST(testGetAddedActiveObjectsAroundPos);
}
SECTION("free ID") {
TestServerActiveObjectMgr saomgr;
std::vector<u16> aoids;
////////////////////////////////////////////////////////////////////////////////
u16 aoid = saomgr.getFreeId();
// Ensure it's not the same id
REQUIRE(saomgr.getFreeId() != aoid);
void TestServerActiveObjectMgr::testFreeID()
{
server::ActiveObjectMgr saomgr;
std::vector<u16> aoids;
aoids.push_back(aoid);
u16 aoid = saomgr.getFreeId();
// Ensure it's not the same id
UASSERT(saomgr.getFreeId() != aoid);
// Register basic objects, ensure we never found
for (u8 i = 0; i < UINT8_MAX; i++) {
// Register an object
auto sao_u = std::make_unique<MockServerActiveObject>();
auto sao = sao_u.get();
saomgr.registerObject(std::move(sao_u));
aoids.push_back(sao->getId());
aoids.push_back(aoid);
// Ensure next id is not in registered list
REQUIRE(std::find(aoids.begin(), aoids.end(), saomgr.getFreeId()) ==
aoids.end());
}
// Register basic objects, ensure we never found
for (u8 i = 0; i < UINT8_MAX; i++) {
// Register an object
saomgr.clear();
}
SECTION("register object") {
TestServerActiveObjectMgr saomgr;
auto sao_u = std::make_unique<MockServerActiveObject>();
auto sao = sao_u.get();
saomgr.registerObject(std::move(sao_u));
aoids.push_back(sao->getId());
REQUIRE(saomgr.registerObject(std::move(sao_u)));
// Ensure next id is not in registered list
UASSERT(std::find(aoids.begin(), aoids.end(), saomgr.getFreeId()) ==
aoids.end());
u16 id = sao->getId();
auto saoToCompare = saomgr.getActiveObject(id);
REQUIRE(saoToCompare->getId() == id);
REQUIRE(saoToCompare == sao);
sao_u = std::make_unique<MockServerActiveObject>();
sao = sao_u.get();
REQUIRE(saomgr.registerObject(std::move(sao_u)));
REQUIRE(saomgr.getActiveObject(sao->getId()) == sao);
REQUIRE(saomgr.getActiveObject(sao->getId()) != saoToCompare);
saomgr.clear();
}
saomgr.clear();
}
SECTION("remove object") {
TestServerActiveObjectMgr saomgr;
auto sao_u = std::make_unique<MockServerActiveObject>();
auto sao = sao_u.get();
REQUIRE(saomgr.registerObject(std::move(sao_u)));
void TestServerActiveObjectMgr::testRegisterObject()
{
server::ActiveObjectMgr saomgr;
auto sao_u = std::make_unique<MockServerActiveObject>();
auto sao = sao_u.get();
UASSERT(saomgr.registerObject(std::move(sao_u)));
u16 id = sao->getId();
REQUIRE(saomgr.getActiveObject(id) != nullptr);
u16 id = sao->getId();
saomgr.removeObject(sao->getId());
REQUIRE(saomgr.getActiveObject(id) == nullptr);
auto saoToCompare = saomgr.getActiveObject(id);
UASSERT(saoToCompare->getId() == id);
UASSERT(saoToCompare == sao);
sao_u = std::make_unique<MockServerActiveObject>();
sao = sao_u.get();
UASSERT(saomgr.registerObject(std::move(sao_u)));
UASSERT(saomgr.getActiveObject(sao->getId()) == sao);
UASSERT(saomgr.getActiveObject(sao->getId()) != saoToCompare);
saomgr.clear();
}
void TestServerActiveObjectMgr::testRemoveObject()
{
server::ActiveObjectMgr saomgr;
auto sao_u = std::make_unique<MockServerActiveObject>();
auto sao = sao_u.get();
UASSERT(saomgr.registerObject(std::move(sao_u)));
u16 id = sao->getId();
UASSERT(saomgr.getActiveObject(id) != nullptr)
saomgr.removeObject(sao->getId());
UASSERT(saomgr.getActiveObject(id) == nullptr);
saomgr.clear();
}
void TestServerActiveObjectMgr::testGetObjectsInsideRadius()
{
server::ActiveObjectMgr saomgr;
static const v3f sao_pos[] = {
v3f(10, 40, 10),
v3f(740, 100, -304),
v3f(-200, 100, -304),
v3f(740, -740, -304),
v3f(1500, -740, -304),
};
for (const auto &p : sao_pos) {
saomgr.registerObject(std::make_unique<MockServerActiveObject>(nullptr, p));
saomgr.clear();
}
std::vector<ServerActiveObject *> result;
saomgr.getObjectsInsideRadius(v3f(), 50, result, nullptr);
UASSERTCMP(int, ==, result.size(), 1);
SECTION("get objects inside radius") {
server::ActiveObjectMgr saomgr;
static const v3f sao_pos[] = {
v3f(10, 40, 10),
v3f(740, 100, -304),
v3f(-200, 100, -304),
v3f(740, -740, -304),
v3f(1500, -740, -304),
};
result.clear();
saomgr.getObjectsInsideRadius(v3f(), 750, result, nullptr);
UASSERTCMP(int, ==, result.size(), 2);
for (const auto &p : sao_pos) {
saomgr.registerObject(std::make_unique<MockServerActiveObject>(nullptr, p));
}
result.clear();
saomgr.getObjectsInsideRadius(v3f(), 750000, result, nullptr);
UASSERTCMP(int, ==, result.size(), 5);
std::vector<ServerActiveObject *> result;
saomgr.getObjectsInsideRadius(v3f(), 50, result, nullptr);
CHECK(result.size() == 1);
result.clear();
auto include_obj_cb = [](ServerActiveObject *obj) {
return (obj->getBasePosition().X != 10);
};
result.clear();
saomgr.getObjectsInsideRadius(v3f(), 750, result, nullptr);
CHECK(result.size() == 2);
saomgr.getObjectsInsideRadius(v3f(), 750000, result, include_obj_cb);
UASSERTCMP(int, ==, result.size(), 4);
result.clear();
saomgr.getObjectsInsideRadius(v3f(), 750000, result, nullptr);
CHECK(result.size() == 5);
saomgr.clear();
}
result.clear();
auto include_obj_cb = [](ServerActiveObject *obj) {
return (obj->getBasePosition().X != 10);
};
void TestServerActiveObjectMgr::testGetAddedActiveObjectsAroundPos()
{
server::ActiveObjectMgr saomgr;
static const v3f sao_pos[] = {
v3f(10, 40, 10),
v3f(740, 100, -304),
v3f(-200, 100, -304),
v3f(740, -740, -304),
v3f(1500, -740, -304),
};
saomgr.getObjectsInsideRadius(v3f(), 750000, result, include_obj_cb);
CHECK(result.size() == 4);
for (const auto &p : sao_pos) {
saomgr.registerObject(std::make_unique<MockServerActiveObject>(nullptr, p));
saomgr.clear();
}
std::vector<u16> result;
std::set<u16> cur_objects;
saomgr.getAddedActiveObjectsAroundPos(v3f(), "singleplayer", 100, 50, cur_objects, result);
UASSERTCMP(int, ==, result.size(), 1);
SECTION("get added active objects around pos") {
server::ActiveObjectMgr saomgr;
static const v3f sao_pos[] = {
v3f(10, 40, 10),
v3f(740, 100, -304),
v3f(-200, 100, -304),
v3f(740, -740, -304),
v3f(1500, -740, -304),
};
result.clear();
cur_objects.clear();
saomgr.getAddedActiveObjectsAroundPos(v3f(), "singleplayer", 740, 50, cur_objects, result);
UASSERTCMP(int, ==, result.size(), 2);
for (const auto &p : sao_pos) {
saomgr.registerObject(std::make_unique<MockServerActiveObject>(nullptr, p));
}
saomgr.clear();
}
std::vector<u16> result;
std::set<u16> cur_objects;
saomgr.getAddedActiveObjectsAroundPos(v3f(), "singleplayer", 100, 50, cur_objects, result);
CHECK(result.size() == 1);
result.clear();
cur_objects.clear();
saomgr.getAddedActiveObjectsAroundPos(v3f(), "singleplayer", 740, 50, cur_objects, result);
CHECK(result.size() == 2);
saomgr.clear();
}
SECTION("spatial index") {
TestServerActiveObjectMgr saomgr;
std::mt19937 gen(0xABCDEF);
std::uniform_int_distribution<s32> coordinate(-1000, 1000);
const auto random_pos = [&]() {
return v3f(coordinate(gen), coordinate(gen), coordinate(gen));
};
std::uniform_int_distribution<u32> percent(0, 99);
const auto modify = [&](u32 p_insert, u32 p_delete, u32 p_update) {
const auto p = percent(gen);
if (p < p_insert) {
saomgr.registerObject(std::make_unique<MockServerActiveObject>(nullptr, random_pos()));
} else if (p < p_insert + p_delete) {
if (!saomgr.empty())
saomgr.removeObject(saomgr.randomId(gen));
} else if (p < p_insert + p_delete + p_update) {
if (!saomgr.empty())
saomgr.updatePos(saomgr.randomId(gen), random_pos());
}
};
const auto test_queries = [&]() {
std::uniform_real_distribution<f32> radius(0, 100);
saomgr.compareObjectsInsideRadius(random_pos(), radius(gen));
aabb3f box(random_pos(), random_pos());
box.repair();
saomgr.compareObjectsInArea(box);
};
// Grow: Insertion twice as likely as deletion
for (u32 i = 0; i < 3000; ++i) {
modify(50, 25, 25);
test_queries();
}
// Stagnate: Insertion and deletion equally likely
for (u32 i = 0; i < 3000; ++i) {
modify(25, 25, 50);
test_queries();
}
// Shrink: Deletion twice as likely as insertion
while (!saomgr.empty()) {
modify(25, 50, 25);
test_queries();
}
}
}

489
src/util/k_d_tree.h Normal file
View File

@ -0,0 +1,489 @@
// 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.
* A special ray proximity query could be implemented. This is tricky however.
*/
using Idx = uint16_t;
// 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;
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());
}
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;
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;
};