Implemented a QuadTree and a test

src/QuadTree/QuadTree.c

@@ -1 +1,428 @@
 #include "QuadTree.h"
+
+#include "../errorcodes.h"
+#include <math.h>
+#include <stdbool.h>
+#include <time.h>
+
+enum QuadTreePosition {
+	POSITION_TOP_LEFT,
+	POSITION_TOP_RIGHT,
+	POSITION_BOTTOM_LEFT,
+	POSITION_BOTTOM_RIGHT,
+};
+
+int QuadTree_Create(QuadTree* tree, scalar width, scalar height, allocator_t* allocator)
+{
+	tree->root = NULL;
+	tree->width = width;
+	tree->height = height;
+	tree->allocator = allocator;
+
+	return EXIT_SUCCESS;
+}
+
+static enum QuadTreePosition Dimension_LeafPosition(QuadTreeLeaf* leaf, QuadTreeDimension dimension)
+{
+	scalar middle_x = (dimension.right - dimension.left) / 2 + dimension.left;
+	scalar middle_y = (dimension.bottom - dimension.top) / 2 + dimension.top;
+
+	if (middle_x < leaf->x) {
+		if (middle_y < leaf->y) {
+			return POSITION_BOTTOM_RIGHT;
+		} else {
+			return POSITION_TOP_RIGHT;
+		}
+	} else {
+		if (middle_y < leaf->y) {
+			return POSITION_BOTTOM_LEFT;
+		} else {
+			return POSITION_TOP_LEFT;
+		}
+	}
+}
+
+static enum QuadTreeNodeType QuadSubTree_GetType(QuadSubTree* tree, enum QuadTreePosition position)
+{
+	switch (position) {
+	case POSITION_TOP_LEFT:
+		return tree->top_left_type;
+
+	case POSITION_TOP_RIGHT:
+		return tree->top_right_type;
+
+	case POSITION_BOTTOM_LEFT:
+		return tree->bottom_left_type;
+
+	case POSITION_BOTTOM_RIGHT:
+		return tree->bottom_right_type;
+	default:
+		return *(int*) NULL;
+        }
+}
+
+static void QuadSubTree_SetType(QuadSubTree* tree, enum QuadTreePosition position, enum QuadTreeNodeType type)
+{
+	switch (position) {
+
+	case POSITION_TOP_LEFT:
+		tree->top_left_type = type;
+		break;
+
+	case POSITION_TOP_RIGHT:
+		tree->top_right_type = type;
+		break;
+
+	case POSITION_BOTTOM_LEFT:
+		tree->bottom_left_type = type;
+		break;
+
+	case POSITION_BOTTOM_RIGHT:
+		tree->bottom_right_type = type;
+		break;
+        }
+}
+
+static void QuadSubTree_SetTree(QuadSubTree* tree, enum QuadTreePosition position, QuadSubTree* branch)
+{
+	QuadSubTree_SetType(tree, position, QUADTREENODETYPE_TREE);
+
+	switch (position) {
+
+	case POSITION_TOP_LEFT:
+		tree->top_left.tree = branch;
+		break;
+
+	case POSITION_TOP_RIGHT:
+		tree->top_right.tree = branch;
+		break;
+
+	case POSITION_BOTTOM_LEFT:
+		tree->bottom_left.tree = branch;
+		break;
+
+	case POSITION_BOTTOM_RIGHT:
+		tree->bottom_right.tree = branch;
+		break;
+	
+        }
+}
+
+static QuadSubTree** QuadSubTree_GetTree(QuadSubTree* tree, enum QuadTreePosition position)
+{
+	switch (position) {
+
+	case POSITION_TOP_LEFT:
+		return &tree->top_left.tree;
+
+	case POSITION_TOP_RIGHT:
+		return &tree->top_right.tree;
+
+	case POSITION_BOTTOM_LEFT:
+		return &tree->bottom_left.tree;
+
+	case POSITION_BOTTOM_RIGHT:
+		return &tree->bottom_right.tree;
+	
+	default:
+		return NULL;
+        }
+}
+
+static void QuadSubTree_SetLeaf(QuadSubTree* tree, enum QuadTreePosition position, QuadTreeLeaf* leaf)
+{
+	QuadSubTree_SetType(tree, position, QUADTREENODETYPE_LEAF);
+
+	switch (position) {
+
+	case POSITION_TOP_LEFT:
+		tree->top_left.leaf = leaf;
+		break;
+
+	case POSITION_TOP_RIGHT:
+		tree->top_right.leaf = leaf;
+		break;
+
+	case POSITION_BOTTOM_LEFT:
+		tree->bottom_left.leaf = leaf;
+		break;
+
+	case POSITION_BOTTOM_RIGHT:
+		tree->bottom_right.leaf = leaf;
+		break;
+        }
+}
+
+static QuadTreeLeaf* QuadSubTree_GetLeaf(QuadSubTree* tree, enum QuadTreePosition position)
+{
+	QuadSubTree_SetType(tree, position, QUADTREENODETYPE_LEAF);
+
+	switch (position) {
+
+	case POSITION_TOP_LEFT:
+		return tree->top_left.leaf;
+
+	case POSITION_TOP_RIGHT:
+		return tree->top_right.leaf;
+
+	case POSITION_BOTTOM_LEFT:
+		return tree->bottom_left.leaf;
+
+	case POSITION_BOTTOM_RIGHT:
+		return tree->bottom_right.leaf;
+	
+	default:
+		return NULL;
+        }
+}
+
+static QuadTreeDimension Dimension_Adjust(QuadTreeDimension dimension, enum QuadTreePosition position)
+{
+	scalar middle_x = (dimension.right - dimension.left) / 2 + dimension.left;
+	scalar middle_y = (dimension.bottom - dimension.top) / 2 + dimension.top;
+
+	switch (position) {
+
+	case POSITION_TOP_LEFT:
+		dimension.bottom = middle_y;
+		dimension.right = middle_x;
+		break;
+
+	case POSITION_TOP_RIGHT:
+		dimension.left = middle_x;
+		dimension.bottom = middle_y;
+		break;
+
+	case POSITION_BOTTOM_LEFT:
+		dimension.right = middle_x;
+		dimension.top = middle_y;
+		break;
+
+	case POSITION_BOTTOM_RIGHT:
+		dimension.left = middle_x;
+		dimension.top = middle_y;
+		break;
+        }
+
+	return dimension;
+}
+
+void QuadSubTree_Create(QuadSubTree* branch)
+{
+	branch->bottom_right_type = QUADTREENODETYPE_EMPTY;
+	branch->top_right_type = QUADTREENODETYPE_EMPTY;
+	branch->bottom_left_type = QUADTREENODETYPE_EMPTY;
+	branch->top_left_type = QUADTREENODETYPE_EMPTY;
+}
+
+bool QuadTreeLeaf_Equal(QuadTreeLeaf* self, QuadTreeLeaf* other)
+{
+	return self->y == other->y && self->x == other->x;
+}
+
+static int QuadSubTree_Insert(QuadSubTree** root, QuadTreeLeaf* leaf, QuadTreeDimension dimension, allocator_t* allocator)
+{
+	if (*root == NULL) {
+		QuadSubTree* branch = Allocator_Allocate(allocator, sizeof(QuadSubTree));
+		if (branch == NULL) return ENOMEM;
+		QuadSubTree_Create(branch);
+		
+		enum QuadTreePosition selected_position = Dimension_LeafPosition(leaf, dimension);
+		QuadSubTree_SetLeaf(branch, selected_position, leaf);
+
+		*root = branch;
+	} else {
+		enum QuadTreePosition branch_position = Dimension_LeafPosition(leaf, dimension);
+		enum QuadTreeNodeType branch_type = QuadSubTree_GetType(*root, branch_position);
+		QuadTreeDimension branch_dimension = Dimension_Adjust(dimension, branch_position);
+		switch (branch_type) {
+			case QUADTREENODETYPE_LEAF:
+			{
+				QuadTreeLeaf* preserved_leaf = QuadSubTree_GetLeaf(*root, branch_position);
+				if (QuadTreeLeaf_Equal(preserved_leaf, leaf)) return EEXIST;
+
+				QuadSubTree_SetTree(*root, branch_position, NULL);
+				QuadSubTree** branch_tree = QuadSubTree_GetTree(*root, branch_position);
+
+				int insert_code = QuadSubTree_Insert(branch_tree, leaf, branch_dimension, allocator);
+				if (insert_code) return insert_code;
+			
+				return QuadSubTree_Insert(branch_tree, preserved_leaf, branch_dimension, allocator);
+			}
+
+			case QUADTREENODETYPE_TREE:
+			{
+				QuadSubTree** branch = QuadSubTree_GetTree(*root, branch_position);
+				return QuadSubTree_Insert(branch, leaf, branch_dimension, allocator);
+			}
+
+                        case QUADTREENODETYPE_EMPTY:
+				QuadSubTree_SetLeaf(*root, branch_position, leaf);
+				break;
+		}
+        }
+
+	return EXIT_SUCCESS;
+}
+
+int QuadTree_Insert(QuadTree* tree, QuadTreeLeaf* leaf)
+{
+	QuadTreeDimension root_dimension = { 0, 0, tree->width, tree->height };
+	return QuadSubTree_Insert(&tree->root, leaf, root_dimension, tree->allocator);
+}
+
+static int QuadSubTree_ForEachTree(QuadSubTree* tree, QuadTree_TreeCallback action, void* context, QuadTreeDimension dimension)
+{
+	// TODO: Fix me
+	if (tree->bottom_left_type == QUADTREENODETYPE_TREE) {
+		QuadTreeDimension d = Dimension_Adjust(dimension, POSITION_BOTTOM_LEFT);
+		int r = QuadSubTree_ForEachTree(tree->bottom_left.tree, action, context, d);
+		if (r) return r;
+	}
+	if (tree->bottom_right_type == QUADTREENODETYPE_TREE) {
+		QuadTreeDimension d = Dimension_Adjust(dimension, POSITION_BOTTOM_RIGHT);
+		int r = QuadSubTree_ForEachTree(tree->bottom_right.tree, action, context, d);
+		if (r) return r;
+	}
+	if (tree->top_left_type == QUADTREENODETYPE_TREE) {
+		QuadTreeDimension d = Dimension_Adjust(dimension, POSITION_TOP_LEFT);
+		int r = QuadSubTree_ForEachTree(tree->top_left.tree, action, context, d);
+		if (r) return r;
+	}
+	if (tree->top_right_type == QUADTREENODETYPE_TREE) {
+		QuadTreeDimension d = Dimension_Adjust(dimension, POSITION_TOP_RIGHT);
+		int r = QuadSubTree_ForEachTree(tree->top_right.tree, action, context, d);
+		if (r) return r;
+	}
+
+	int action_result = action(context, tree, dimension);
+	if (action_result) return action_result;
+
+	return EXIT_SUCCESS;
+}
+
+int QuadTree_ForEachTree(QuadTree* tree, QuadTree_TreeCallback action, void* context)
+{
+	QuadTreeDimension root_dimension = { 0, 0, tree->width, tree->height };
+	return QuadSubTree_ForEachTree(tree->root, action, context, root_dimension);
+}
+
+int QuadSubTree_ForEachLeaf(QuadSubTree* tree, QuadTree_LeafCallback action, void* context)
+{
+	enum QuadTreePosition positions[] = {
+		POSITION_TOP_LEFT,
+		POSITION_TOP_RIGHT,
+		POSITION_BOTTOM_LEFT,
+		POSITION_BOTTOM_RIGHT,
+	};
+
+	for (size_t i = 0; i < sizeof(positions) / sizeof(positions[0]); i++) {
+		enum QuadTreeNodeType type = QuadSubTree_GetType(tree, positions[i]);
+		switch (type) {
+            case QUADTREENODETYPE_TREE:
+			{
+				QuadSubTree** subtree = QuadSubTree_GetTree(tree, positions[i]);
+				int result = QuadSubTree_ForEachLeaf(*subtree, action, context);
+				if (result != EXIT_SUCCESS) return result;
+				break;
+			}
+            case QUADTREENODETYPE_LEAF:
+			{
+				QuadTreeLeaf* leaf = QuadSubTree_GetLeaf(tree, positions[i]);
+				int result = action(context, leaf);
+				if (result != EXIT_SUCCESS) return result;
+				break;
+			}
+            case QUADTREENODETYPE_EMPTY:
+				break;
+		}
+    }
+
+	return EXIT_SUCCESS;
+}
+
+int QuadTree_ForEachLeaf(QuadTree* tree, QuadTree_LeafCallback action, void* context)
+{
+	return QuadSubTree_ForEachLeaf(tree->root, action, context);
+}
+
+bool Dimension_IsInRadiusOfLeaf(QuadTreeDimension dimension, QuadTreeLeaf point, scalar radius)
+{
+	// is inside x radius?
+	if (true
+		&& (point.x + radius) > dimension.left
+		&& (point.x - radius) < dimension.right
+	) {
+		// is inside y radius?
+		if (true
+			&& (point.y + radius) > dimension.top
+			&& (point.y - radius) < dimension.bottom
+		) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
+int QuadSubTree_ForEachLeafInRadius(QuadSubTree* tree, QuadTree_LeafCallback action, void* context, QuadTreeDimension dimension, QuadTreeLeaf point, scalar radius)
+{
+	enum QuadTreePosition positions[] = {
+		POSITION_TOP_LEFT,
+		POSITION_TOP_RIGHT,
+		POSITION_BOTTOM_LEFT,
+		POSITION_BOTTOM_RIGHT,
+	};
+
+	for (size_t i = 0; i < sizeof(positions) / sizeof(positions[0]); i++) {
+		enum QuadTreePosition position = positions[i];
+		enum QuadTreeNodeType type = QuadSubTree_GetType(tree, position);
+		switch (type) {
+			case QUADTREENODETYPE_TREE:
+			{
+				QuadTreeDimension branch_dimension = Dimension_Adjust(dimension, position);
+				QuadSubTree** branch = QuadSubTree_GetTree(tree, position);
+				if (Dimension_IsInRadiusOfLeaf(branch_dimension, point, radius)) {
+					int result = QuadSubTree_ForEachLeafInRadius(*branch, action, context, branch_dimension, point, radius);
+					if (result != EXIT_SUCCESS) return result;
+				}
+				break;
+			}
+			case QUADTREENODETYPE_LEAF:
+			{
+				QuadTreeLeaf* leaf = QuadSubTree_GetLeaf(tree, position);
+				if (QuadTreeLeaf_Distance(&point, leaf) <= radius) {
+					int result = action(context, leaf);
+					if (result != EXIT_SUCCESS) return result;
+				}
+				break;
+			}
+			case QUADTREENODETYPE_EMPTY:
+				break;
+		}
+	}
+
+	return EXIT_SUCCESS;
+}
+
+scalar QuadTreeLeaf_Distance(QuadTreeLeaf* self, QuadTreeLeaf* other)
+{
+	return sqrt(pow(self->x - other->x, 2) + pow(self->y - other->y, 2));
+}
+
+int QuadTree_ForEachLeafInRadius(QuadTree* tree, QuadTree_LeafCallback action, void* context, QuadTreeLeaf point, scalar radius)
+{
+	QuadTreeDimension root_dimension = { 0, 0, tree->width, tree->height };
+	return QuadSubTree_ForEachLeafInRadius(tree->root, action, context, root_dimension, point, radius);
+}
+
+int QuadSubTree_Destroy(void* allocator, QuadSubTree* tree, QuadTreeDimension dimension)
+{
+	return Allocator_Free(allocator, tree, sizeof(*tree));
+}
+
+void QuadTree_Destroy(QuadTree* tree)
+{
+	QuadTree_ForEachTree(tree, QuadSubTree_Destroy, tree->allocator);
+}
+

src/QuadTree/QuadTree.h

@@ -1,8 +1,66 @@
 #ifndef UTILITIEC_QUADTREE_H
 #define UTILITIEC_QUADTREE_H
 
+#include <stddef.h>
+
+#include "../allocator-interface/allocator-interface.h"
+
+typedef double scalar;
+
+enum QuadTreeNodeType {
+	QUADTREENODETYPE_TREE,
+	QUADTREENODETYPE_LEAF,
+	QUADTREENODETYPE_EMPTY,
+};
+
+typedef struct QuadSubTree_s QuadSubTree;
+
+typedef struct QuadTreeLeaf {
+	scalar x;
+	scalar y;
+} QuadTreeLeaf;
+
+union QuadTreeNode {
+	QuadSubTree* tree;
+	QuadTreeLeaf* leaf;
+};
+
+struct QuadSubTree_s {
+	enum QuadTreeNodeType top_left_type     :2;
+	enum QuadTreeNodeType top_right_type    :2;
+	enum QuadTreeNodeType bottom_left_type  :2;
+	enum QuadTreeNodeType bottom_right_type :2;
+	union QuadTreeNode top_left;
+	union QuadTreeNode top_right;
+	union QuadTreeNode bottom_left;
+	union QuadTreeNode bottom_right;
+};
+
 typedef struct QuadTree_s {
-	
+	QuadSubTree* root;
+	allocator_t* allocator;
+	scalar width;
+	scalar height;
 } QuadTree;
 
+int QuadTree_Create(QuadTree* tree, scalar width, scalar height, allocator_t* allocator);
+int QuadTree_Insert(QuadTree* tree, QuadTreeLeaf* leaf);
+
+typedef struct QuadTreeDimension_s {
+	scalar top;
+	scalar left;
+	scalar right;
+	scalar bottom;
+} QuadTreeDimension;
+
+typedef int (*QuadTree_TreeCallback) (void* context, QuadSubTree* tree, QuadTreeDimension dimension);
+int QuadTree_ForEachTree(QuadTree* tree, QuadTree_TreeCallback action, void* context);
+
+typedef int (*QuadTree_LeafCallback) (void* context, QuadTreeLeaf* leaf);
+int QuadTree_ForEachLeaf(QuadTree* tree, QuadTree_LeafCallback action, void* context);
+int QuadTree_ForEachLeafInRadius(QuadTree* tree, QuadTree_LeafCallback action, void* context, QuadTreeLeaf point, scalar radius);
+void QuadTree_Destroy(QuadTree* tree);
+
+scalar QuadTreeLeaf_Distance(QuadTreeLeaf* self, QuadTreeLeaf* other);
+
 #endif

tests/CMakeLists.txt

@@ -52,3 +52,11 @@ target_link_libraries(Subprocess-test
 	pointers
 	allocator-interface
 )
+
+add_executable(QuadTree-test QuadTree.test.c)
+target_link_libraries(QuadTree-test
+	QuadTree
+	rand
+	pointers
+	allocator-interface
+)

tests/QuadTree.test.c

@@ -0,0 +1,25 @@
+#include "../src/rand/xoshiro256.h"
+#include <stdio.h>
+#include "../src/QuadTree/QuadTree.h"
+
+static QuadTree tree;
+
+typedef struct Particle_s {
+        QuadTreeLeaf position;
+} Particle;
+
+static Particle particles[100];
+
+int main()
+{
+        QuadTree_Create(&tree, 10000, 10000, NULL);
+
+        Xoshiro256State rand_state = { 0, 10, 0, 0 };
+
+        for (size_t i = 0; i < sizeof(particles) / sizeof(particles[0]); i++) {
+                particles[i].position.x = xoshiro256_next(&rand_state) % 10000;
+                particles[i].position.y = xoshiro256_next(&rand_state) % 10000;
+                printf("%f %f\n", particles[i].position.y, particles[i].position.x);
+                QuadTree_Insert(&tree, &particles[i].position);
+        }
+}