From bcbe072b664807e1603ba3bf85b7357bca33d773 Mon Sep 17 00:00:00 2001
From: Lars Mueller <appgurulars@gmx.de>
Date: Thu, 9 May 2024 16:44:10 +0200
Subject: [PATCH] Add failing test

---
 src/unittest/test_k_d_tree.cpp | 37 +++++++++++++++++++++++++++++++++-
 src/util/k_d_tree.h            | 33 +++++++++++++++++++-----------
 2 files changed, 57 insertions(+), 13 deletions(-)

diff --git a/src/unittest/test_k_d_tree.cpp b/src/unittest/test_k_d_tree.cpp
index 924a61423..f22b5877d 100644
--- a/src/unittest/test_k_d_tree.cpp
+++ b/src/unittest/test_k_d_tree.cpp
@@ -17,6 +17,7 @@ class TestKdTree : public TestBase
 	void runTests(IGameDef *gamedef);
 
 	// TODO basic small cube test
+	void singleUpdate();
 	void randomOps();
 };
 
@@ -34,6 +35,10 @@ class ObjectVector {
 		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) {
@@ -57,18 +62,40 @@ 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;
-	for (u16 id = 1; id < 1000; ++id) {
+
+	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);
 	}
@@ -101,4 +128,12 @@ void TestKdTree::randomOps() {
 	}
 
 	testRandomQueries();
+
+	for (u16 id = 800; id < 1000; ++id) {
+		const auto point = randPos();
+		objvec.update(point, id);
+		kds.update(point, id);
+	}
+
+	testRandomQueries();
 }
\ No newline at end of file
diff --git a/src/util/k_d_tree.h b/src/util/k_d_tree.h
index 487cb903f..5a9c68526 100644
--- a/src/util/k_d_tree.h
+++ b/src/util/k_d_tree.h
@@ -25,6 +25,7 @@ class Points {
 	//! 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) {
@@ -116,7 +117,10 @@ class SortedIndices {
 			auto right_ptr = right.indices.begin(d);
 			for (auto it = indices.begin(d); it != indices.end(d); ++it) {
 				if (*it != *mid) { // ignore pivot
-					*(markers[*it] ? left_ptr++ : right_ptr++) = *it;
+					if (markers[*it])
+						*(left_ptr++) = *it;
+					else
+						*(right_ptr++) = *it;
 				}
 			}
 		}
@@ -333,6 +337,7 @@ class KdTree {
 	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>;
@@ -340,28 +345,28 @@ class DynamicKdTrees {
 	using Point = typename Tree::Point;
 	void insert(const std::array<Component, Dim> &point, const Id id) {
 		Tree tree(point, id);
-		for (uint8_t treeIdx = 0;; ++treeIdx) {
-			if (treeIdx >= trees.size()) {
-				tree.foreach([&](Idx objIdx, auto _, Id id) {
-					del_entries[id] = {treeIdx, objIdx};
+		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[treeIdx].empty()) {
+			if (trees[tree_idx].empty()) {
 				// TODO deduplicate
-				tree.foreach([&](Idx objIdx, auto _, Id id) {
-					del_entries[id] = {treeIdx, objIdx};
+				tree.foreach([&](Idx in_tree_idx, auto _, Id id) {
+					del_entries[id] = {tree_idx, in_tree_idx};
 				});
-				trees[treeIdx] = std::move(tree);
+				trees[tree_idx] = std::move(tree);
 				break;
 			}
-			tree = Tree(tree, trees[treeIdx]);
-			trees[treeIdx] = std::move(Tree());
+			tree = Tree(tree, trees[tree_idx]);
+			trees[tree_idx] = std::move(Tree());
 		}
 		++n_entries;
 	}
-	void remove(const Id id) {
+	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...
@@ -369,6 +374,10 @@ class DynamicKdTrees {
 		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 {