Implemented a QuadTree and a test
This commit is contained in:
parent
a569dd05a6
commit
87f592ea8b
4 changed files with 519 additions and 1 deletions
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
)
|
||||
|
|
|
|||
25
tests/QuadTree.test.c
Normal file
25
tests/QuadTree.test.c
Normal file
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
Loading…
Reference in a new issue