diff --git a/src/QuadTree/QuadTree.c b/src/QuadTree/QuadTree.c index 6042bc2..a6b8d6f 100644 --- a/src/QuadTree/QuadTree.c +++ b/src/QuadTree/QuadTree.c @@ -1 +1,428 @@ #include "QuadTree.h" + +#include "../errorcodes.h" +#include +#include +#include + +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); +} + diff --git a/src/QuadTree/QuadTree.h b/src/QuadTree/QuadTree.h index 00470e8..a4c118e 100644 --- a/src/QuadTree/QuadTree.h +++ b/src/QuadTree/QuadTree.h @@ -1,8 +1,66 @@ #ifndef UTILITIEC_QUADTREE_H #define UTILITIEC_QUADTREE_H +#include + +#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 diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 95df5c2..3089073 100644 --- a/tests/CMakeLists.txt +++ b/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 +) diff --git a/tests/QuadTree.test.c b/tests/QuadTree.test.c new file mode 100644 index 0000000..9c391fa --- /dev/null +++ b/tests/QuadTree.test.c @@ -0,0 +1,25 @@ +#include "../src/rand/xoshiro256.h" +#include +#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); + } +}