diff --git a/meson.build b/meson.build index c869ee25..1cf242c0 100644 --- a/meson.build +++ b/meson.build @@ -89,7 +89,7 @@ if get_option('enet') endif sources = [] -src += ['anim.c', 'config.c', 'datastream.c','font.c','gameobject.c','HandmadeMath.c','jsffi.c','model.c','render.c','script.c','simplex.c','spline.c', 'timer.c', 'transform.c','warp.c','yugine.c', 'wildmatch.c', 'sprite.c', 'quadtree.c', 'aabb.c'] +src += ['anim.c', 'config.c', 'datastream.c','font.c','gameobject.c','HandmadeMath.c','jsffi.c','model.c','render.c','script.c','simplex.c','spline.c', 'timer.c', 'transform.c','warp.c','yugine.c', 'wildmatch.c', 'sprite.c', 'quadtree.c', 'aabb.c', 'Quadtree.c', 'IntList.c', 'rtree.c'] imsrc = ['GraphEditor.cpp','ImCurveEdit.cpp','ImGradient.cpp','imgui_draw.cpp','imgui_tables.cpp','imgui_widgets.cpp','imgui.cpp','ImGuizmo.cpp','imnodes.cpp','implot_items.cpp','implot.cpp', 'imgui_impl_sdlrenderer3.cpp', 'imgui_impl_sdl3.cpp', 'imgui_impl_sdlgpu3.cpp'] diff --git a/scripts/components.js b/scripts/components.js index 6f8255b7..b7ea1cb7 100644 --- a/scripts/components.js +++ b/scripts/components.js @@ -341,7 +341,9 @@ var world = {x:-1000,y:-1000, width:3000, height:3000}; //globalThis.sprite_qt = new Quadtree({x:-1000,y:-1000, width:3000, height:3000}, 10); //globalThis.sprite_qt = new LooseQuadtree(world, 10, 1.2); //globalThis.sprite_qt = new RTree(10) -globalThis.sprite_qt = os.make_quadtree(world, 4); +//globalThis.sprite_qt = os.make_quadtree(world, 4); +//globalThis.sprite_qt = os.make_qtree(world); +globalThis.sprite_qt = os.make_rtree(); var spritetree = os.make_quadtree([0,0], [10000,10000]); globalThis.spritetree = spritetree; diff --git a/source/IntList.c b/source/IntList.c new file mode 100644 index 00000000..daef4b33 --- /dev/null +++ b/source/IntList.c @@ -0,0 +1,118 @@ +/* + * This code was initially authored by the Stackoverflow user dragon-energy and posted under following page: + * https://stackoverflow.com/questions/41946007/efficient-and-well-explained-implementation-of-a-quadtree-for-2d-collision-det + * + * As for the license, the author has kindly noted: + * + * "Oh and feel free to use this code I post however you want, even for commercial projects. + * I would really love it if people let me know if they find it useful, but do as you wish." + * + * And generally all Stackoverflow-posted code is by default licensed with CC BY-SA 4.0: + * https://creativecommons.org/licenses/by-sa/4.0/ + */ + +#include "IntList.h" +#include +#include +#include + +void il_create(IntList* il, int num_fields) +{ + il->data = il->fixed; + il->num = 0; + il->cap = il_fixed_cap; + il->num_fields = num_fields; + il->free_element = -1; +} + +void il_destroy(IntList* il) +{ + // Free the buffer only if it was heap allocated. + if (il->data != il->fixed) + free(il->data); +} + +void il_clear(IntList* il) +{ + il->num = 0; + il->free_element = -1; +} + +int il_size(const IntList* il) +{ + return il->num; +} + +int il_get(const IntList* il, int n, int field) +{ + assert(n >= 0 && n < il->num); + return il->data[n*il->num_fields + field]; +} + +void il_set(IntList* il, int n, int field, int val) +{ + assert(n >= 0 && n < il->num); + il->data[n*il->num_fields + field] = val; +} + +int il_push_back(IntList* il) +{ + const int new_pos = (il->num+1) * il->num_fields; + + // If the list is full, we need to reallocate the buffer to make room + // for the new element. + if (new_pos > il->cap) + { + // Use double the size for the new capacity. + const int new_cap = new_pos * 2; + + // If we're pointing to the fixed buffer, allocate a new array on the + // heap and copy the fixed buffer contents to it. + if (il->cap == il_fixed_cap) + { + il->data = malloc(new_cap * sizeof(*il->data)); + memcpy(il->data, il->fixed, sizeof(il->fixed)); + } + else + { + // Otherwise reallocate the heap buffer to the new size. + il->data = realloc(il->data, new_cap * sizeof(*il->data)); + } + // Set the old capacity to the new capacity. + il->cap = new_cap; + } + return il->num++; +} + +void il_pop_back(IntList* il) +{ + // Just decrement the list size. + assert(il->num > 0); + --il->num; +} + +int il_insert(IntList* il) +{ + // If there's a free index in the free list, pop that and use it. + if (il->free_element != -1) + { + const int index = il->free_element; + const int pos = index * il->num_fields; + + // Set the free index to the next free index. + il->free_element = il->data[pos]; + + // Return the free index. + return index; + } + // Otherwise insert to the back of the array. + return il_push_back(il); +} + +void il_erase(IntList* il, int n) +{ + // Push the element to the free list. + const int pos = n * il->num_fields; + il->data[pos] = il->free_element; + il->free_element = n; +} \ No newline at end of file diff --git a/source/IntList.h b/source/IntList.h new file mode 100644 index 00000000..ee685ad8 --- /dev/null +++ b/source/IntList.h @@ -0,0 +1,90 @@ +/* + * This code was initially authored by the Stackoverflow user dragon-energy and posted under following page: + * https://stackoverflow.com/questions/41946007/efficient-and-well-explained-implementation-of-a-quadtree-for-2d-collision-det + * + * As for the license, the author has kindly noted: + * + * "Oh and feel free to use this code I post however you want, even for commercial projects. + * I would really love it if people let me know if they find it useful, but do as you wish." + * + * And generally all Stackoverflow-posted code is by default licensed with CC BY-SA 4.0: + * https://creativecommons.org/licenses/by-sa/4.0/ + */ + +#ifndef INT_LIST_H +#define INT_LIST_H + +#ifdef __cplusplus +#define IL_FUNC extern "C" +#else +#define IL_FUNC +#endif + +typedef struct IntList IntList; +enum {il_fixed_cap = 128}; + +struct IntList +{ + // Stores a fixed-size buffer in advance to avoid requiring + // a heap allocation until we run out of space. + int fixed[il_fixed_cap]; + + // Points to the buffer used by the list. Initially this will + // point to 'fixed'. + int* data; + + // Stores how many integer fields each element has. + int num_fields; + + // Stores the number of elements in the list. + int num; + + // Stores the capacity of the array. + int cap; + + // Stores an index to the free element or -1 if the free list + // is empty. + int free_element; +}; + +// --------------------------------------------------------------------------------- +// List Interface +// --------------------------------------------------------------------------------- +// Creates a new list of elements which each consist of integer fields. +// 'num_fields' specifies the number of integer fields each element has. +IL_FUNC void il_create(IntList* il, int num_fields); + +// Destroys the specified list. +IL_FUNC void il_destroy(IntList* il); + +// Returns the number of elements in the list. +IL_FUNC int il_size(const IntList* il); + +// Returns the value of the specified field for the nth element. +IL_FUNC int il_get(const IntList* il, int n, int field); + +// Sets the value of the specified field for the nth element. +IL_FUNC void il_set(IntList* il, int n, int field, int val); + +// Clears the specified list, making it empty. +IL_FUNC void il_clear(IntList* il); + +// --------------------------------------------------------------------------------- +// Stack Interface (do not mix with free list usage; use one or the other) +// --------------------------------------------------------------------------------- +// Inserts an element to the back of the list and returns an index to it. +IL_FUNC int il_push_back(IntList* il); + +// Removes the element at the back of the list. +IL_FUNC void il_pop_back(IntList* il); + +// --------------------------------------------------------------------------------- +// Free List Interface (do not mix with stack usage; use one or the other) +// --------------------------------------------------------------------------------- +// Inserts an element to a vacant position in the list and returns an index to it. +IL_FUNC int il_insert(IntList* il); + +// Removes the nth element in the list. +IL_FUNC void il_erase(IntList* il, int n); + +#endif \ No newline at end of file diff --git a/source/Quadtree.c b/source/Quadtree.c new file mode 100644 index 00000000..15407ea0 --- /dev/null +++ b/source/Quadtree.c @@ -0,0 +1,457 @@ +/* + * This code was initially authored by the Stackoverflow user dragon-energy and posted under following page: + * https://stackoverflow.com/questions/41946007/efficient-and-well-explained-implementation-of-a-quadtree-for-2d-collision-det + * + * As for the license, the author has kindly noted: + * + * "Oh and feel free to use this code I post however you want, even for commercial projects. + * I would really love it if people let me know if they find it useful, but do as you wish." + * + * And generally all Stackoverflow-posted code is by default licensed with CC BY-SA 4.0: + * https://creativecommons.org/licenses/by-sa/4.0/ + */ + +#include "Quadtree.h" +#include +#include "string.h" + +enum +{ + // ---------------------------------------------------------------------------------------- + // Element node fields: + // ---------------------------------------------------------------------------------------- + enode_num = 2, + + // Points to the next element in the leaf node. A value of -1 + // indicates the end of the list. + enode_idx_next = 0, + + // Stores the element index. + enode_idx_elt = 1, + + // ---------------------------------------------------------------------------------------- + // Element fields: + // ---------------------------------------------------------------------------------------- + elt_num = 5, + + // Stores the rectangle encompassing the element. + elt_idx_lft = 0, elt_idx_top = 1, elt_idx_rgt = 2, elt_idx_btm = 3, + + // Stores the ID of the element. + elt_idx_id = 4, + + // ---------------------------------------------------------------------------------------- + // Node fields: + // ---------------------------------------------------------------------------------------- + node_num = 2, + + // Points to the first child if this node is a branch or the first element + // if this node is a leaf. + node_idx_fc = 0, + + // Stores the number of elements in the node or -1 if it is not a leaf. + node_idx_num = 1, + + // ---------------------------------------------------------------------------------------- + // Node data fields: + // ---------------------------------------------------------------------------------------- + nd_num = 6, + + // Stores the extents of the node using a centered rectangle and half-size. + nd_idx_mx = 0, nd_idx_my = 1, nd_idx_sx = 2, nd_idx_sy = 3, + + // Stores the index of the node. + nd_idx_index = 4, + + // Stores the depth of the node. + nd_idx_depth = 5, +}; + +static void node_insert(Quadtree* qt, int index, int depth, int mx, int my, int sx, int sy, int element); + +static int intersect(int l1, int t1, int r1, int b1, + int l2, int t2, int r2, int b2) +{ + return l2 <= r1 && r2 >= l1 && t2 <= b1 && b2 >= t1; +} + +void leaf_insert(Quadtree* qt, int node, int depth, int mx, int my, int sx, int sy, int element) +{ + // Insert the element node to the leaf. + const int nd_fc = il_get(&qt->nodes, node, node_idx_fc); + il_set(&qt->nodes, node, node_idx_fc, il_insert(&qt->enodes)); + il_set(&qt->enodes, il_get(&qt->nodes, node, node_idx_fc), enode_idx_next, nd_fc); + il_set(&qt->enodes, il_get(&qt->nodes, node, node_idx_fc), enode_idx_elt, element); + + // If the leaf is full, split it. + if (il_get(&qt->nodes, node, node_idx_num) == qt->max_elements && depth < qt->max_depth) + { + int fc = 0, j = 0; + IntList elts = {0}; + il_create(&elts, 1); + + // Transfer elements from the leaf node to a list of elements. + while (il_get(&qt->nodes, node, node_idx_fc) != -1) + { + const int index = il_get(&qt->nodes, node, node_idx_fc); + const int next_index = il_get(&qt->enodes, index, enode_idx_next); + const int elt = il_get(&qt->enodes, index, enode_idx_elt); + + // Pop off the element node from the leaf and remove it from the qt. + il_set(&qt->nodes, node, node_idx_fc, next_index); + il_erase(&qt->enodes, index); + + // Insert element to the list. + il_set(&elts, il_push_back(&elts), 0, elt); + } + + // Start by allocating 4 child nodes. + fc = il_insert(&qt->nodes); + il_insert(&qt->nodes); + il_insert(&qt->nodes); + il_insert(&qt->nodes); + il_set(&qt->nodes, node, node_idx_fc, fc); + + // Initialize the new child nodes. + for (j=0; j < 4; ++j) + { + il_set(&qt->nodes, fc+j, node_idx_fc, -1); + il_set(&qt->nodes, fc+j, node_idx_num, 0); + } + + // Transfer the elements in the former leaf node to its new children. + il_set(&qt->nodes, node, node_idx_num, -1); + for (j=0; j < il_size(&elts); ++j) + node_insert(qt, node, depth, mx, my, sx, sy, il_get(&elts, j, 0)); + il_destroy(&elts); + } + else + { + // Increment the leaf element count. + il_set(&qt->nodes, node, node_idx_num, il_get(&qt->nodes, node, node_idx_num) + 1); + } +} + +static void push_node(IntList* nodes, int nd_index, int nd_depth, int nd_mx, int nd_my, int nd_sx, int nd_sy) +{ + const int back_idx = il_push_back(nodes); + il_set(nodes, back_idx, nd_idx_mx, nd_mx); + il_set(nodes, back_idx, nd_idx_my, nd_my); + il_set(nodes, back_idx, nd_idx_sx, nd_sx); + il_set(nodes, back_idx, nd_idx_sy, nd_sy); + il_set(nodes, back_idx, nd_idx_index, nd_index); + il_set(nodes, back_idx, nd_idx_depth, nd_depth); +} + +static void find_leaves(IntList* out, const Quadtree* qt, int node, int depth, + int mx, int my, int sx, int sy, + int lft, int top, int rgt, int btm) +{ + IntList to_process = {0}; + il_create(&to_process, nd_num); + push_node(&to_process, node, depth, mx, my, sx, sy); + + while (il_size(&to_process) > 0) + { + const int back_idx = il_size(&to_process) - 1; + const int nd_mx = il_get(&to_process, back_idx, nd_idx_mx); + const int nd_my = il_get(&to_process, back_idx, nd_idx_my); + const int nd_sx = il_get(&to_process, back_idx, nd_idx_sx); + const int nd_sy = il_get(&to_process, back_idx, nd_idx_sy); + const int nd_index = il_get(&to_process, back_idx, nd_idx_index); + const int nd_depth = il_get(&to_process, back_idx, nd_idx_depth); + il_pop_back(&to_process); + + // If this node is a leaf, insert it to the list. + if (il_get(&qt->nodes, nd_index, node_idx_num) != -1) + push_node(out, nd_index, nd_depth, nd_mx, nd_my, nd_sx, nd_sy); + else + { + // Otherwise push the children that intersect the rectangle. + const int fc = il_get(&qt->nodes, nd_index, node_idx_fc); + const int hx = nd_sx >> 1, hy = nd_sy >> 1; + const int l = nd_mx-hx, t = nd_my-hy, r = nd_mx+hx, b = nd_my+hy; + + if (top <= nd_my) + { + if (lft <= nd_mx) + push_node(&to_process, fc+0, nd_depth+1, l,t,hx,hy); + if (rgt > nd_mx) + push_node(&to_process, fc+1, nd_depth+1, r,t,hx,hy); + } + if (btm > nd_my) + { + if (lft <= nd_mx) + push_node(&to_process, fc+2, nd_depth+1, l,b,hx,hy); + if (rgt > nd_mx) + push_node(&to_process, fc+3, nd_depth+1, r,b,hx,hy); + } + } + } + il_destroy(&to_process); +} + +static void node_insert(Quadtree* qt, int index, int depth, int mx, int my, int sx, int sy, int element) +{ + // Find the leaves and insert the element to all the leaves found. + int j = 0; + IntList leaves = {0}; + + const int lft = il_get(&qt->elts, element, elt_idx_lft); + const int top = il_get(&qt->elts, element, elt_idx_top); + const int rgt = il_get(&qt->elts, element, elt_idx_rgt); + const int btm = il_get(&qt->elts, element, elt_idx_btm); + + il_create(&leaves, nd_num); + find_leaves(&leaves, qt, index, depth, mx, my, sx, sy, lft, top, rgt, btm); + for (j=0; j < il_size(&leaves); ++j) + { + const int nd_mx = il_get(&leaves, j, nd_idx_mx); + const int nd_my = il_get(&leaves, j, nd_idx_my); + const int nd_sx = il_get(&leaves, j, nd_idx_sx); + const int nd_sy = il_get(&leaves, j, nd_idx_sy); + const int nd_index = il_get(&leaves, j, nd_idx_index); + const int nd_depth = il_get(&leaves, j, nd_idx_depth); + leaf_insert(qt, nd_index, nd_depth, nd_mx, nd_my, nd_sx, nd_sy, element); + } + il_destroy(&leaves); +} + +void qt_create(Quadtree* qt, int x1, int y1, int x2, int y2, int max_elements, int max_depth) +{ + qt->max_elements = max_elements; + qt->max_depth = max_depth; + qt->temp = 0; + qt->temp_size = 0; + il_create(&qt->nodes, node_num); + il_create(&qt->elts, elt_num); + il_create(&qt->enodes, enode_num); + + // Insert the root node to the qt. + il_insert(&qt->nodes); + il_set(&qt->nodes, 0, node_idx_fc, -1); + il_set(&qt->nodes, 0, node_idx_num, 0); + + int half_width = (x2-x1) >> 1; + int half_height = (y2-y1) >> 1; + qt->root_sx = half_width; + qt->root_sy = half_height; + + // Center + qt->root_mx = x1 + half_width; + qt->root_my = y1 + half_height; +} + +void qt_destroy(Quadtree* qt) +{ + il_destroy(&qt->nodes); + il_destroy(&qt->elts); + il_destroy(&qt->enodes); + free(qt->temp); +} + +int qt_insert(Quadtree* qt, int id, int x1, int y1, int x2, int y2) +{ + // Insert a new element. + const int new_element = il_insert(&qt->elts); + + // Set the fields of the new element. + il_set(&qt->elts, new_element, elt_idx_lft, x1); + il_set(&qt->elts, new_element, elt_idx_top, y1); + il_set(&qt->elts, new_element, elt_idx_rgt, x2); + il_set(&qt->elts, new_element, elt_idx_btm, y2); + il_set(&qt->elts, new_element, elt_idx_id, id); + + // Insert the element to the appropriate leaf node(s). + node_insert(qt, 0, 0, qt->root_mx, qt->root_my, qt->root_sx, qt->root_sy, new_element); + return new_element; +} + +void qt_remove(Quadtree* qt, int element) +{ + // Find the leaves. + int j = 0; + IntList leaves = {0}; + + const int lft = il_get(&qt->elts, element, elt_idx_lft); + const int top = il_get(&qt->elts, element, elt_idx_top); + const int rgt = il_get(&qt->elts, element, elt_idx_rgt); + const int btm = il_get(&qt->elts, element, elt_idx_btm); + + il_create(&leaves, nd_num); + find_leaves(&leaves, qt, 0, 0, qt->root_mx, qt->root_my, qt->root_sx, qt->root_sy, lft, top, rgt, btm); + + // For each leaf node, remove the element node. + for (j=0; j < il_size(&leaves); ++j) + { + const int nd_index = il_get(&leaves, j, nd_idx_index); + + // Walk the list until we find the element node. + int node_index = il_get(&qt->nodes, nd_index, node_idx_fc); + int prev_index = -1; + while (node_index != -1 && il_get(&qt->enodes, node_index, enode_idx_elt) != element) + { + prev_index = node_index; + node_index = il_get(&qt->enodes, node_index, enode_idx_next); + } + + if (node_index != -1) + { + // Remove the element node. + const int next_index = il_get(&qt->enodes, node_index, enode_idx_next); + if (prev_index == -1) + il_set(&qt->nodes, nd_index, node_idx_fc, next_index); + else + il_set(&qt->enodes, prev_index, enode_idx_next, next_index); + il_erase(&qt->enodes, node_index); + + // Decrement the leaf element count. + il_set(&qt->nodes, nd_index, node_idx_num, il_get(&qt->nodes, nd_index, node_idx_num)-1); + } + } + il_destroy(&leaves); + + // Remove the element. + il_erase(&qt->elts, element); +} + +void qt_query(Quadtree* qt, IntList* out, int qlft, int qtop, int qrgt, int qbtm, int omit_element) +{ + // Find the leaves that intersect the specified query rectangle. + int j = 0; + IntList leaves = {0}; + const int elt_cap = il_size(&qt->elts); + + if (qt->temp_size < elt_cap) + { + qt->temp_size = elt_cap; + qt->temp = realloc(qt->temp, qt->temp_size * sizeof(*qt->temp)); + memset(qt->temp, 0, qt->temp_size * sizeof(*qt->temp)); + } + + // For each leaf node, look for elements that intersect. + il_create(&leaves, nd_num); + find_leaves(&leaves, qt, 0, 0, qt->root_mx, qt->root_my, qt->root_sx, qt->root_sy, qlft, qtop, qrgt, qbtm); + + il_clear(out); + for (j=0; j < il_size(&leaves); ++j) + { + const int nd_index = il_get(&leaves, j, nd_idx_index); + + // Walk the list and add elements that intersect. + int elt_node_index = il_get(&qt->nodes, nd_index, node_idx_fc); + while (elt_node_index != -1) + { + const int element = il_get(&qt->enodes, elt_node_index, enode_idx_elt); + const int lft = il_get(&qt->elts, element, elt_idx_lft); + const int top = il_get(&qt->elts, element, elt_idx_top); + const int rgt = il_get(&qt->elts, element, elt_idx_rgt); + const int btm = il_get(&qt->elts, element, elt_idx_btm); + if (!qt->temp[element] && element != omit_element && intersect(qlft,qtop,qrgt,qbtm, lft,top,rgt,btm)) + { + il_set(out, il_push_back(out), 0, element); + qt->temp[element] = 1; + } + elt_node_index = il_get(&qt->enodes, elt_node_index, enode_idx_next); + } + } + il_destroy(&leaves); + + // Unmark the elements that were inserted. + for (j=0; j < il_size(out); ++j) + qt->temp[il_get(out, j, 0)] = 0; +} + +void qt_cleanup(Quadtree* qt) +{ + IntList to_process = {0}; + il_create(&to_process, 1); + + // Only process the root if it's not a leaf. + if (il_get(&qt->nodes, 0, node_idx_num) == -1) + { + // Push the root index to the stack. + il_set(&to_process, il_push_back(&to_process), 0, 0); + } + + while (il_size(&to_process) > 0) + { + // Pop a node from the stack. + const int node = il_get(&to_process, il_size(&to_process)-1, 0); + const int fc = il_get(&qt->nodes, node, node_idx_fc); + int num_empty_leaves = 0; + int j = 0; + il_pop_back(&to_process); + + // Loop through the children. + for (j=0; j < 4; ++j) + { + const int child = fc + j; + + // Increment empty leaf count if the child is an empty + // leaf. Otherwise if the child is a branch, add it to + // the stack to be processed in the next iteration. + if (il_get(&qt->nodes, child, node_idx_num) == 0) + ++num_empty_leaves; + else if (il_get(&qt->nodes, child, node_idx_num) == -1) + { + // Push the child index to the stack. + il_set(&to_process, il_push_back(&to_process), 0, child); + } + } + + // If all the children were empty leaves, remove them and + // make this node the new empty leaf. + if (num_empty_leaves == 4) + { + // Remove all 4 children in reverse order so that they + // can be reclaimed on subsequent insertions in proper + // order. + il_erase(&qt->nodes, fc + 3); + il_erase(&qt->nodes, fc + 2); + il_erase(&qt->nodes, fc + 1); + il_erase(&qt->nodes, fc + 0); + + // Make this node the new empty leaf. + il_set(&qt->nodes, node, node_idx_fc, -1); + il_set(&qt->nodes, node, node_idx_num, 0); + } + } + il_destroy(&to_process); +} + +void qt_traverse(Quadtree* qt, void* user_data, QtNodeFunc* branch, QtNodeFunc* leaf) +{ + IntList to_process = {0}; + il_create(&to_process, nd_num); + push_node(&to_process, 0, 0, qt->root_mx, qt->root_my, qt->root_sx, qt->root_sy); + + while (il_size(&to_process) > 0) + { + const int back_idx = il_size(&to_process) - 1; + const int nd_mx = il_get(&to_process, back_idx, nd_idx_mx); + const int nd_my = il_get(&to_process, back_idx, nd_idx_my); + const int nd_sx = il_get(&to_process, back_idx, nd_idx_sx); + const int nd_sy = il_get(&to_process, back_idx, nd_idx_sy); + const int nd_index = il_get(&to_process, back_idx, nd_idx_index); + const int nd_depth = il_get(&to_process, back_idx, nd_idx_depth); + const int fc = il_get(&qt->nodes, nd_index, node_idx_fc); + il_pop_back(&to_process); + + if (il_get(&qt->nodes, nd_index, node_idx_num) == -1) + { + // Push the children of the branch to the stack. + const int hx = nd_sx >> 1, hy = nd_sy >> 1; + const int l = nd_mx-hx, t = nd_my-hy, r = nd_mx+hx, b = nd_my+hy; + push_node(&to_process, fc+0, nd_depth+1, l,t, hx,hy); + push_node(&to_process, fc+1, nd_depth+1, r,t, hx,hy); + push_node(&to_process, fc+2, nd_depth+1, l,b, hx,hy); + push_node(&to_process, fc+3, nd_depth+1, r,b, hx,hy); + if (branch) + branch(qt, user_data, nd_index, nd_depth, nd_mx, nd_my, nd_sx, nd_sy); + } + else if (leaf) + leaf(qt, user_data, nd_index, nd_depth, nd_mx, nd_my, nd_sx, nd_sy); + } + il_destroy(&to_process); +} \ No newline at end of file diff --git a/source/Quadtree.h b/source/Quadtree.h new file mode 100644 index 00000000..caf31ba0 --- /dev/null +++ b/source/Quadtree.h @@ -0,0 +1,83 @@ +/* + * This code was initially authored by the Stackoverflow user dragon-energy and posted under following page: + * https://stackoverflow.com/questions/41946007/efficient-and-well-explained-implementation-of-a-quadtree-for-2d-collision-det + * + * As for the license, the author has kindly noted: + * + * "Oh and feel free to use this code I post however you want, even for commercial projects. + * I would really love it if people let me know if they find it useful, but do as you wish." + * + * And generally all Stackoverflow-posted code is by default licensed with CC BY-SA 4.0: + * https://creativecommons.org/licenses/by-sa/4.0/ + */ + +#ifndef QUADTREE_H +#define QUADTREE_H + +#include "IntList.h" + +#ifdef __cplusplus +#define QTREE_FUNC extern "C" +#else +#define QTREE_FUNC +#endif + +typedef struct Quadtree Quadtree; + +struct Quadtree +{ + // Stores all the nodes in the quadtree. The first node in this + // sequence is always the root. + IntList nodes; + + // Stores all the elements in the quadtree. + IntList elts; + + // Stores all the element nodes in the quadtree. + IntList enodes; + + // Stores the quadtree extents. + int root_mx, root_my, root_sx, root_sy; + + // Maximum allowed elements in a leaf before the leaf is subdivided/split unless + // the leaf is at the maximum allowed tree depth. + int max_elements; + + // Stores the maximum depth allowed for the quadtree. + int max_depth; + + // Temporary buffer used for queries. + + char* temp; + + // Stores the size of the temporary buffer. + int temp_size; +}; + +// Function signature used for traversing a tree node. +typedef void QtNodeFunc(Quadtree* qt, void* user_data, int node, int depth, int mx, int my, int sx, int sy); + +// Creates a quadtree with the requested extents, maximum elements per leaf, and maximum tree depth. +QTREE_FUNC void qt_create(Quadtree* qt, int x1, int y1, int x2, int y2, int max_elements, int max_depth); + +// Destroys the quadtree. +QTREE_FUNC void qt_destroy(Quadtree* qt); + +// Inserts a new element to the tree. +// Returns an index to the new element. +QTREE_FUNC int qt_insert(Quadtree* qt, int id, int x1, int y1, int x2, int y2); + +// Removes the specified element from the tree. +QTREE_FUNC void qt_remove(Quadtree* qt, int element); + +// Cleans up the tree, removing empty leaves. +QTREE_FUNC void qt_cleanup(Quadtree* qt); + +// Outputs a list of elements found in the specified rectangle. +QTREE_FUNC void qt_query(Quadtree* qt, IntList* out, int x1, int y1, int x2, int y2, int omit_element); + +// Traverses all the nodes in the tree, calling 'branch' for branch nodes and 'leaf' +// for leaf nodes. +QTREE_FUNC void qt_traverse(Quadtree* qt, void* user_data, QtNodeFunc* branch, QtNodeFunc* leaf); + +#endif \ No newline at end of file diff --git a/source/jsffi.c b/source/jsffi.c index e27e0aaa..abda9b37 100644 --- a/source/jsffi.c +++ b/source/jsffi.c @@ -29,9 +29,15 @@ #include "cgltf.h" #include "physfs.h" +#include "freelist.h" + #include "sprite.h" #include "quadtree.h" +#include "Quadtree.h" +#include "rtree.h" + +typedef struct rtree rtree; #include #include @@ -366,6 +372,8 @@ struct lrtb { float b; }; +static JSContext *global_js; + static SDL_GPUDevice *global_gpu; SDL_GPUGraphicsPipelineTargetInfo js2SDL_GPUGraphicsPipelineTargetInfo(JSContext *js, JSValue v) @@ -1157,6 +1165,20 @@ void qtree_free(JSRuntime *rt, qtree *tree) QJSCLASS(qtree) +void Quadtree_free(JSRuntime *rt, Quadtree *tree) +{ + qt_destroy(tree); +} + +QJSCLASS(Quadtree) + +void rtree_free(JSRuntime *rt, rtree *tree) +{ + rtree_destroy(tree); +} + +QJSCLASS(rtree) + int js_arrlen(JSContext *js,JSValue v) { if (JS_IsUndefined(v)) return 0; int len; @@ -7122,12 +7144,35 @@ int js_qtree_cmp(struct qtree_sprite *v, aabb *range) return SDL_HasRectIntersectionFloat(&v->rect, &ab); } +int js_qtree_rm(struct qtree_sprite *val, struct qtree_sprite *cmp) +{ + int same = JS_SameValue(global_js, val->value, cmp->value); + if (same) + JS_FreeValue(global_js, val->value); + return same; +} + JSC_CCALL(os_make_quadtree, rect area = js2rect(js,argv[0]); - qtree tree = qtree_new(area.x,area.y,area.w,area.h, js_qtree_cmp); + qtree tree = qtree_new(area.x,area.y,area.w,area.h, js_qtree_cmp, js_qtree_rm); return qtree2js(js,tree); ) +struct { int key; JSValue value; } *qthash = NULL; + +JSC_CCALL(os_make_qtree, + hmdefault(qthash, JS_UNDEFINED); + rect area = js2rect(js,argv[0]); + Quadtree *tree = malloc(sizeof(*tree)); + qt_create(tree, area.x, area.y, area.x+area.w, area.y+area.h, 4, 8); + return Quadtree2js(js,tree); +) + +JSC_CCALL(os_make_rtree, + struct rtree *tree = rtree_new(); + return rtree2js(js,tree); +) + static const JSCFunctionListEntry js_os_funcs[] = { MIST_FUNC_DEF(os, turbulence, 4), MIST_FUNC_DEF(os, model_buffer, 1), @@ -7145,6 +7190,8 @@ static const JSCFunctionListEntry js_os_funcs[] = { MIST_FUNC_DEF(os, gc, 0), MIST_FUNC_DEF(os, eval, 2), MIST_FUNC_DEF(os, make_quadtree, 1), + MIST_FUNC_DEF(os, make_qtree, 1), + MIST_FUNC_DEF(os, make_rtree, 0), MIST_FUNC_DEF(os, make_texture, 1), MIST_FUNC_DEF(os, make_gif, 1), MIST_FUNC_DEF(os, make_aseprite, 1), @@ -7203,8 +7250,10 @@ JSC_CCALL(qtree_insert, ) JSC_CCALL(qtree_remove, + struct qtree_sprite tmp; + tmp.value = argv[0]; qtree tree = js2qtree(js,self); - JSValue item = argv[0]; + qtree_remove(tree, &tmp); ) JSC_CCALL(qtree_query, @@ -7226,6 +7275,131 @@ static const JSCFunctionListEntry js_qtree_funcs[] = { MIST_FUNC_DEF(qtree, query, 2), }; + + +JSC_CCALL(Quadtree_insert, + Quadtree *tree = js2Quadtree(js,self); + JSValue v = argv[0]; + rect r; + JS_GETATOM(js,r,v,rect_atom,rect) + int id = (int)JS_VALUE_GET_PTR(v); + int e = qt_insert(tree, id, r.x,r.y,r.x+r.w,r.y+r.h); + hmput(qthash, e, JS_DupValue(js,v)); +) + +JSC_CCALL(Quadtree_remove, + return JS_UNDEFINED; + Quadtree *tree = js2Quadtree(js,self); + int e = js2number(js,argv[0]); + qt_remove(tree, e); + // free jsvalue +) + +JSC_CCALL(Quadtree_query, + Quadtree *tree = js2Quadtree(js,self); + rect area = js2rect(js,argv[0]); + IntList out; + il_create(&out,1); + qt_query(tree,&out,area.x,area.y,area.x+area.w,area.y+area.h,-1); + + ret = JS_NewArray(js); + + int n = 0; + for (int i = 0; i < il_size(&out); i++) { + int id = il_get(&out, i, 0); + JSValue v = hmget(qthash, id); + if (!JS_IsUndefined(v)) + JS_SetPropertyUint32(js,ret,n++,JS_DupValue(js,v)); + } +) + +static const JSCFunctionListEntry js_Quadtree_funcs[] = { + MIST_FUNC_DEF(Quadtree, insert, 1), + MIST_FUNC_DEF(Quadtree, remove, 1), + MIST_FUNC_DEF(Quadtree, query, 1), +}; + +JSC_CCALL(rtree_insert, + rtree *tree = js2rtree(js,self); + JSValue v = argv[0]; + rect r; + JS_GETATOM(js,r,v,rect_atom,rect) + double min[2]; + double max[2]; + min[0] = r.x; + min[1] = r.y; + max[0] = r.x+r.w; + max[1] = r.y+r.h; + JSValue *ins = malloc(sizeof(*ins)); + *ins = JS_DupValue(js,v); + rtree_insert(tree, min, max, ins); +) + +int rtree_cmp(const JSValue *a, const JSValue *b, JSContext *js) +{ + int same = JS_SameValue(js, *a, *b); + if (same) + JS_FreeValue(js,*a); + + return !same; +} + +JSC_CCALL(rtree_remove, + rtree *tree = js2rtree(js,self); + JSValue v = argv[0]; + rect r; + JS_GETATOM(js,r,v,rect_atom,rect) + double min[2]; + double max[2]; + min[0] = r.x; + min[1] = r.y; + max[0] = r.x+r.w; + max[1] = r.y+r.h; + + int deleted = rtree_delete_with_comparator(tree, min, max, &v, rtree_cmp, js); +) + +struct rtree_iter_data { + JSContext *js; + JSValue arr; + int n; +}; + +bool rtree_iter(const double *min, const double *max, const JSValue *data, struct rtree_iter_data *ctx) +{ + JS_SetPropertyUint32(ctx->js,ctx->arr,ctx->n, JS_DupValue(ctx->js,*data)); + ctx->n++; +} + +JSC_CCALL(rtree_query, + rtree *tree = js2rtree(js,self); + rect r = js2rect(js,argv[0]); + double min[2]; + double max[2]; + min[0] = r.x; + min[1] = r.y; + max[0] = r.x+r.w; + max[1] = r.y+r.h; + struct rtree_iter_data data = {0}; + data.js = js; + data.arr = JS_NewArray(js); + data.n = 0; + rtree_search(tree, min, max, rtree_iter, &data); + ret = data.arr; +) + +JSC_CCALL(rtree_count, + rtree *tree = js2rtree(js,self); + return number2js(js, rtree_count(tree)); +) + +static const JSCFunctionListEntry js_rtree_funcs[] = { + MIST_FUNC_DEF(rtree, insert, 1), + MIST_FUNC_DEF(rtree, remove, 1), + MIST_FUNC_DEF(rtree, query, 1), + MIST_FUNC_DEF(rtree, count, 0), +}; + static const JSCFunctionListEntry js_jssprite_funcs[] = { }; @@ -7287,6 +7461,8 @@ void ffi_load(JSContext *js) { JSValue globalThis = JS_GetGlobalObject(js); QJSCLASSPREP_FUNCS(qtree) + QJSCLASSPREP_FUNCS(Quadtree) + QJSCLASSPREP_FUNCS(rtree) QJSCLASSPREP_FUNCS(SDL_Window) QJSCLASSPREP_FUNCS(SDL_Surface) QJSCLASSPREP_FUNCS(SDL_Thread) @@ -7441,6 +7617,10 @@ void ffi_load(JSContext *js) { rect_atom = JS_NewAtom(js,"rect"); fill_event_atoms(js); + + global_js = js; + + JSValue *vals = NULL; JS_FreeValue(js,globalThis); } diff --git a/source/quadtree.c b/source/quadtree.c index 93029dc7..0c635c4d 100644 --- a/source/quadtree.c +++ b/source/quadtree.c @@ -1,11 +1,3 @@ -/* - quadtree.c - 2014 JSK (kutani@projectkutani.com) - - Part of the Panic Panic project. - - Released to the public domain. See LICENSE for details. -*/ #include #include #include @@ -18,6 +10,7 @@ /// A function pointer def for determining if an element exists in a range typedef int (*qtree_fnc)(void *ptr, aabb *range); +typedef int (*qtree_rm)(void *ptr, void *cmp); /// Quadtree node typedef struct qnode { @@ -35,6 +28,7 @@ typedef struct _qtree { uint16_t maxnodecap; ///< Maximum element count per node qnode *root; ///< Root node qtree_fnc cmpfnc; ///< Element range compare function pointer + qtree_rm rmfnc; } _qtree; typedef struct _qtree* qtree; @@ -46,22 +40,19 @@ typedef struct retlist { void **list; ///< Array of pointers to found elements } retlist; -static void -retlist_add(retlist *r, void *p) { +static void retlist_add(retlist *r, void *p) { r->list = realloc(r->list, sizeof(void*)*(r->cnt+1)); r->list[r->cnt] = p; r->cnt++; } -static uint16_t -qtree_getMaxNodeCnt(qtree q) { +static uint16_t qtree_getMaxNodeCnt(qtree q) { uint16_t r; r = q->maxnodecap; return r; } -static qnode* -qnode_new(qtree p, float x, float y, float hW, float hH) { +static qnode* qnode_new(qtree p, float x, float y, float hW, float hH) { qnode *q = malloc(sizeof(qnode)); memset(q, 0, sizeof(qnode)); q->bound.center.x = x; @@ -72,9 +63,7 @@ qnode_new(qtree p, float x, float y, float hW, float hH) { return q; } -static void -qnode_free(qtree q, qnode *qn) { - +static void qnode_free(qtree q, qnode *qn) { if(qn->cnt) free(qn->elist); @@ -90,15 +79,13 @@ qnode_free(qtree q, qnode *qn) { free(qn); } -static void -add(qnode *q, void *p) { +static void add(qnode *q, void *p) { q->elist = realloc(q->elist, sizeof(void*)*(q->cnt+1)); q->elist[q->cnt] = p; q->cnt++; } -static void -drop(qnode *q, uint16_t idx) { +static void drop(qnode *q, uint16_t idx) { void **narry = malloc(sizeof(void*)*(q->cnt-1)); // This is a little (lot) ugly; a pair of memcpy's would be @@ -114,8 +101,7 @@ drop(qnode *q, uint16_t idx) { q->cnt--; } -static void -subdivide(qtree p, qnode *q) { +static void subdivide(qtree p, qnode *q) { float cx = q->bound.center.x; float cy = q->bound.center.y; float hw = q->bound.dims.w/2; @@ -127,8 +113,7 @@ subdivide(qtree p, qnode *q) { q->se = qnode_new(p, cx+hw, cy+hh, hw, hh); } -static int -qnode_insert(qtree q, qnode *qn, void *ptr) { +static int qnode_insert(qtree q, qnode *qn, void *ptr) { int ret = 0; if(! (q->cmpfnc)(ptr, &qn->bound)) return 0; @@ -151,11 +136,10 @@ qnode_insert(qtree q, qnode *qn, void *ptr) { return 1; } -static void* -qnode_remove(qtree q, qnode *qn, void *ptr) { +static void* qnode_remove(qtree q, qnode *qn, void *ptr) { if(qn->cnt) { for(uint16_t i=0; icnt; i++) { - if(qn->elist[i] == ptr) { + if(q->rmfnc(qn->elist[i], ptr)) { drop(qn, i); ptr = NULL; goto QN_REM_EXIT; @@ -176,8 +160,7 @@ qnode_remove(qtree q, qnode *qn, void *ptr) { return ptr; } -static void -qnode_getInRange(qtree q, qnode *qn, retlist *r) { +static void qnode_getInRange(qtree q, qnode *qn, retlist *r) { if(qn->cnt) { if(! aabb_intersects(&qn->bound, &r->range)) goto QN_GET_EXIT; @@ -199,22 +182,19 @@ qnode_getInRange(qtree q, qnode *qn, retlist *r) { return; } -/* exports */ - -qtree -qtree_new(float x, float y, float w, float h, qtree_fnc fnc) { +qtree qtree_new(float x, float y, float w, float h, qtree_fnc fnc, qtree_rm rm) { qtree q = malloc(sizeof(_qtree)); memset(q, 0, sizeof(_qtree)); q->maxnodecap = QTREE_STDCAP; q->cmpfnc = fnc; + q->rmfnc = rm; q->root = qnode_new(q, x+(w/2),y+(h/2),w/2,h/2); return q; } -void -qtree_destroy(qtree q) { +void qtree_destroy(qtree q) { void *m; if(q->root) qnode_free(q, q->root); @@ -223,23 +203,19 @@ qtree_destroy(qtree q) { free(q); } -void -qtree_insert(qtree q, void *ptr) { +void qtree_insert(qtree q, void *ptr) { qnode_insert(q, q->root, ptr); } -void -qtree_remove(qtree q, void *ptr) { +void qtree_remove(qtree q, void *ptr) { qnode_remove(q, q->root, ptr); } -void -qtree_setMaxNodeCnt(qtree q, uint16_t cnt) { +void qtree_setMaxNodeCnt(qtree q, uint16_t cnt) { q->maxnodecap = cnt || 1; } -void -qtree_clear(qtree q) { +void qtree_clear(qtree q) { float x = q->root->bound.center.x; float y = q->root->bound.center.y; float w = q->root->bound.dims.w; @@ -251,8 +227,7 @@ qtree_clear(qtree q) { qnode_free(q, qn); } -void** -qtree_findInArea(qtree q, float x, float y, float w, float h, uint32_t *cnt) { +void** qtree_findInArea(qtree q, float x, float y, float w, float h, uint32_t *cnt) { float hw = w/2; float hh = h/2; diff --git a/source/quadtree.h b/source/quadtree.h index 922a9abd..35b28272 100644 --- a/source/quadtree.h +++ b/source/quadtree.h @@ -19,6 +19,8 @@ typedef struct _qtree* qtree; /// A function pointer def for determining if an element exists in a range typedef int (*qtree_fnc)(void *ptr, aabb *range); +typedef int (*qtree_rm)(void *ptr, void *cmp); + /// Create a new qtree /*! Creates a new qtree with a bound of w,h size, centered at x,y. @@ -28,7 +30,7 @@ typedef int (*qtree_fnc)(void *ptr, aabb *range); Returns a new qtree pointer. */ -qtree qtree_new(float x, float y, float w, float h, qtree_fnc fnc); +qtree qtree_new(float x, float y, float w, float h, qtree_fnc fnc, qtree_rm rm); void qtree_destroy(qtree q); diff --git a/source/rtree.c b/source/rtree.c new file mode 100644 index 00000000..0dcf679b --- /dev/null +++ b/source/rtree.c @@ -0,0 +1,840 @@ +// Copyright 2023 Joshua J Baker. All rights reserved. +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file. + +#include +#include +#include +#include "rtree.h" + +//////////////////////////////// + +#define DATATYPE void * +#define DIMS 2 +#define NUMTYPE double +#define MAXITEMS 64 + +//////////////////////////////// + +// used for splits +#define MINITEMS_PERCENTAGE 10 +#define MINITEMS ((MAXITEMS) * (MINITEMS_PERCENTAGE) / 100 + 1) + +#ifndef RTREE_NOPATHHINT +#define USE_PATHHINT +#endif + +#ifdef RTREE_MAXITEMS +#undef MAXITEMS +#define MAXITEMS RTREE_MAXITEMS +#endif + +#ifdef RTREE_NOATOMICS +typedef int rc_t; +static int rc_load(rc_t *ptr, bool relaxed) { + (void)relaxed; // nothing to do + return *ptr; +} +static int rc_fetch_sub(rc_t *ptr, int val) { + int rc = *ptr; + *ptr -= val; + return rc; +} +static int rc_fetch_add(rc_t *ptr, int val) { + int rc = *ptr; + *ptr += val; + return rc; +} +#else +#include +typedef atomic_int rc_t; +static int rc_load(rc_t *ptr, bool relaxed) { + if (relaxed) { + return atomic_load_explicit(ptr, memory_order_relaxed); + } else { + return atomic_load(ptr); + } +} +static int rc_fetch_sub(rc_t *ptr, int delta) { + return atomic_fetch_sub(ptr, delta); +} +static int rc_fetch_add(rc_t *ptr, int delta) { + return atomic_fetch_add(ptr, delta); +} +#endif + +enum kind { + LEAF = 1, + BRANCH = 2, +}; + +struct rect { + NUMTYPE min[DIMS]; + NUMTYPE max[DIMS]; +}; + +struct item { + const DATATYPE data; +}; + +struct node { + rc_t rc; // reference counter for copy-on-write + enum kind kind; // LEAF or BRANCH + int count; // number of rects + struct rect rects[MAXITEMS]; + union { + struct node *nodes[MAXITEMS]; + struct item datas[MAXITEMS]; + }; +}; + +struct rtree { + struct rect rect; + struct node *root; + size_t count; + size_t height; +#ifdef USE_PATHHINT + int path_hint[16]; +#endif + bool relaxed; + void *(*malloc)(size_t); + void (*free)(void *); + void *udata; + bool (*item_clone)(const DATATYPE item, DATATYPE *into, void *udata); + void (*item_free)(const DATATYPE item, void *udata); +}; + +static inline NUMTYPE min0(NUMTYPE x, NUMTYPE y) { + return x < y ? x : y; +} + +static inline NUMTYPE max0(NUMTYPE x, NUMTYPE y) { + return x > y ? x : y; +} + +static bool feq(NUMTYPE a, NUMTYPE b) { + return !(a < b || a > b); +} + + +void rtree_set_udata(struct rtree *tr, void *udata) { + tr->udata = udata; +} + +static struct node *node_new(struct rtree *tr, enum kind kind) { + struct node *node = (struct node *)tr->malloc(sizeof(struct node)); + if (!node) return NULL; + memset(node, 0, sizeof(struct node)); + node->kind = kind; + return node; +} + +static struct node *node_copy(struct rtree *tr, struct node *node) { + struct node *node2 = (struct node *)tr->malloc(sizeof(struct node)); + if (!node2) return NULL; + memcpy(node2, node, sizeof(struct node)); + node2->rc = 0; + if (node2->kind == BRANCH) { + for (int i = 0; i < node2->count; i++) { + rc_fetch_add(&node2->nodes[i]->rc, 1); + } + } else { + if (tr->item_clone) { + int n = 0; + bool oom = false; + for (int i = 0; i < node2->count; i++) { + if (!tr->item_clone(node->datas[i].data, + (DATATYPE*)&node2->datas[i].data, tr->udata)) + { + oom = true; + break; + } + n++; + } + if (oom) { + if (tr->item_free) { + for (int i = 0; i < n; i++) { + tr->item_free(node2->datas[i].data, tr->udata); + } + } + tr->free(node2); + return NULL; + } + } + } + return node2; +} + +static void node_free(struct rtree *tr, struct node *node) { + if (rc_fetch_sub(&node->rc, 1) > 0) return; + if (node->kind == BRANCH) { + for (int i = 0; i < node->count; i++) { + node_free(tr, node->nodes[i]); + } + } else { + if (tr->item_free) { + for (int i = 0; i < node->count; i++) { + tr->item_free(node->datas[i].data, tr->udata); + } + } + } + tr->free(node); +} + +#define cow_node_or(rnode, code) { \ + if (rc_load(&(rnode)->rc, tr->relaxed) > 0) { \ + struct node *node2 = node_copy(tr, (rnode)); \ + if (!node2) { code; } \ + node_free(tr, rnode); \ + (rnode) = node2; \ + } \ +} + +static void rect_expand(struct rect *rect, const struct rect *other) { + for (int i = 0; i < DIMS; i++) { + rect->min[i] = min0(rect->min[i], other->min[i]); + rect->max[i] = max0(rect->max[i], other->max[i]); + } +} + +static NUMTYPE rect_area(const struct rect *rect) { + NUMTYPE result = 1; + for (int i = 0; i < DIMS; i++) { + result *= (rect->max[i] - rect->min[i]); + } + return result; +} + +// return the area of two rects expanded +static NUMTYPE rect_unioned_area(const struct rect *rect, + const struct rect *other) +{ + NUMTYPE result = 1; + for (int i = 0; i < DIMS; i++) { + result *= (max0(rect->max[i], other->max[i]) - + min0(rect->min[i], other->min[i])); + } + return result; +} + +static bool rect_contains(const struct rect *rect, const struct rect *other) { + int bits = 0; + for (int i = 0; i < DIMS; i++) { + bits |= other->min[i] < rect->min[i]; + bits |= other->max[i] > rect->max[i]; + } + return bits == 0; +} + +static bool rect_intersects(const struct rect *rect, const struct rect *other) { + int bits = 0; + for (int i = 0; i < DIMS; i++) { + bits |= other->min[i] > rect->max[i]; + bits |= other->max[i] < rect->min[i]; + } + return bits == 0; +} + +static bool rect_onedge(const struct rect *rect, const struct rect *other) { + for (int i = 0; i < DIMS; i++) { + if (feq(rect->min[i], other->min[i]) || + feq(rect->max[i], other->max[i])) + { + return true; + } + } + return false; +} + +static bool rect_equals(const struct rect *rect, const struct rect *other) { + for (int i = 0; i < DIMS; i++) { + if (!feq(rect->min[i], other->min[i]) || + !feq(rect->max[i], other->max[i])) + { + return false; + } + } + return true; +} + +static bool rect_equals_bin(const struct rect *rect, const struct rect *other) { + for (int i = 0; i < DIMS; i++) { + if (rect->min[i] != other->min[i] || + rect->max[i] != other->max[i]) + { + return false; + } + } + return true; +} + +static int rect_largest_axis(const struct rect *rect) { + int axis = 0; + NUMTYPE nlength = rect->max[0] - rect->min[0]; + for (int i = 1; i < DIMS; i++) { + NUMTYPE length = rect->max[i] - rect->min[i]; + if (length > nlength) { + nlength = length; + axis = i; + } + } + return axis; +} + +// swap two rectangles +static void node_swap(struct node *node, int i, int j) { + struct rect tmp = node->rects[i]; + node->rects[i] = node->rects[j]; + node->rects[j] = tmp; + if (node->kind == LEAF) { + struct item tmp = node->datas[i]; + node->datas[i] = node->datas[j]; + node->datas[j] = tmp; + } else { + struct node *tmp = node->nodes[i]; + node->nodes[i] = node->nodes[j]; + node->nodes[j] = tmp; + } +} + +struct rect4 { + NUMTYPE all[DIMS*2]; +}; + +static void node_qsort(struct node *node, int s, int e, int index) { + int nrects = e - s; + if (nrects < 2) { + return; + } + int left = 0; + int right = nrects-1; + int pivot = nrects / 2; + node_swap(node, s+pivot, s+right); + struct rect4 *rects = (struct rect4 *)&node->rects[s]; + for (int i = 0; i < nrects; i++) { + if (rects[right].all[index] < rects[i].all[index]) { + node_swap(node, s+i, s+left); + left++; + } + } + node_swap(node, s+left, s+right); + node_qsort(node, s, s+left, index); + node_qsort(node, s+left+1, e, index); +} + +// sort the node rectangles by the axis. used during splits +static void node_sort_by_axis(struct node *node, int axis, bool max) { + int by_index = max ? DIMS+axis : axis; + node_qsort(node, 0, node->count, by_index); +} + +static void node_move_rect_at_index_into(struct node *from, int index, + struct node *into) +{ + into->rects[into->count] = from->rects[index]; + from->rects[index] = from->rects[from->count-1]; + if (from->kind == LEAF) { + into->datas[into->count] = from->datas[index]; + from->datas[index] = from->datas[from->count-1]; + } else { + into->nodes[into->count] = from->nodes[index]; + from->nodes[index] = from->nodes[from->count-1]; + } + from->count--; + into->count++; +} + +static bool node_split_largest_axis_edge_snap(struct rtree *tr, + struct rect *rect, struct node *node, struct node **right_out) +{ + int axis = rect_largest_axis(rect); + struct node *right = node_new(tr, node->kind); + if (!right) { + return false; + } + for (int i = 0; i < node->count; i++) { + NUMTYPE min_dist = node->rects[i].min[axis] - rect->min[axis]; + NUMTYPE max_dist = rect->max[axis] - node->rects[i].max[axis]; + if (max_dist < min_dist) { + // move to right + node_move_rect_at_index_into(node, i, right); + i--; + } + } + // Make sure that both left and right nodes have at least + // MINITEMS by moving datas into underflowed nodes. + if (node->count < MINITEMS) { + // reverse sort by min axis + node_sort_by_axis(right, axis, false); + do { + node_move_rect_at_index_into(right, right->count-1, node); + } while (node->count < MINITEMS); + } else if (right->count < MINITEMS) { + // reverse sort by max axis + node_sort_by_axis(node, axis, true); + do { + node_move_rect_at_index_into(node, node->count-1, right); + } while (right->count < MINITEMS); + } + if (node->kind == BRANCH) { + node_sort_by_axis(node, 0, false); + node_sort_by_axis(right, 0, false); + } + *right_out = right; + return true; +} + +static bool node_split(struct rtree *tr, struct rect *rect, struct node *node, + struct node **right) +{ + return node_split_largest_axis_edge_snap(tr, rect, node, right); +} + +static int node_choose_least_enlargement(const struct node *node, + const struct rect *ir) +{ + int j = 0; + NUMTYPE jenlarge = INFINITY; + for (int i = 0; i < node->count; i++) { + // calculate the enlarged area + NUMTYPE uarea = rect_unioned_area(&node->rects[i], ir); + NUMTYPE area = rect_area(&node->rects[i]); + NUMTYPE enlarge = uarea - area; + if (enlarge < jenlarge) { + j = i; + jenlarge = enlarge; + } + } + return j; +} + +static int node_choose(struct rtree *tr, const struct node *node, + const struct rect *rect, int depth) +{ +#ifdef USE_PATHHINT + int h = tr->path_hint[depth]; + if (h < node->count) { + if (rect_contains(&node->rects[h], rect)) { + return h; + } + } +#endif + // Take a quick look for the first node that contain the rect. + for (int i = 0; i < node->count; i++) { + if (rect_contains(&node->rects[i], rect)) { +#ifdef USE_PATHHINT + tr->path_hint[depth] = i; +#endif + return i; + } + } + // Fallback to using che "choose least enlargment" algorithm. + int i = node_choose_least_enlargement(node, rect); +#ifdef USE_PATHHINT + tr->path_hint[depth] = i; +#endif + return i; +} + +static struct rect node_rect_calc(const struct node *node) { + struct rect rect = node->rects[0]; + for (int i = 1; i < node->count; i++) { + rect_expand(&rect, &node->rects[i]); + } + return rect; +} + +// node_insert returns false if out of memory +static bool node_insert(struct rtree *tr, struct rect *nr, struct node *node, + struct rect *ir, struct item item, int depth, bool *split) +{ + if (node->kind == LEAF) { + if (node->count == MAXITEMS) { + *split = true; + return true; + } + int index = node->count; + node->rects[index] = *ir; + node->datas[index] = item; + node->count++; + *split = false; + return true; + } + // Choose a subtree for inserting the rectangle. + int i = node_choose(tr, node, ir, depth); + cow_node_or(node->nodes[i], return false); + if (!node_insert(tr, &node->rects[i], node->nodes[i], ir, item, depth+1, + split)) + { + return false; + } + if (!*split) { + rect_expand(&node->rects[i], ir); + *split = false; + return true; + } + // split the child node + if (node->count == MAXITEMS) { + *split = true; + return true; + } + struct node *right; + if (!node_split(tr, &node->rects[i], node->nodes[i], &right)) { + return false; + } + node->rects[i] = node_rect_calc(node->nodes[i]); + node->rects[node->count] = node_rect_calc(right); + node->nodes[node->count] = right; + node->count++; + return node_insert(tr, nr, node, ir, item, depth, split); +} + +struct rtree *rtree_new_with_allocator(void *(*_malloc)(size_t), + void (*_free)(void*) +) { + _malloc = _malloc ? _malloc : malloc; + _free = _free ? _free : free; + struct rtree *tr = (struct rtree *)_malloc(sizeof(struct rtree)); + if (!tr) return NULL; + memset(tr, 0, sizeof(struct rtree)); + tr->malloc = _malloc; + tr->free = _free; + return tr; +} + +struct rtree *rtree_new(void) { + return rtree_new_with_allocator(NULL, NULL); +} + +void rtree_set_item_callbacks(struct rtree *tr, + bool (*clone)(const DATATYPE item, DATATYPE *into, void *udata), + void (*free)(const DATATYPE item, void *udata)) +{ + tr->item_clone = clone; + tr->item_free = free; +} + +bool rtree_insert(struct rtree *tr, const NUMTYPE *min, + const NUMTYPE *max, const DATATYPE data) +{ + // copy input rect + struct rect rect; + memcpy(&rect.min[0], min, sizeof(NUMTYPE)*DIMS); + memcpy(&rect.max[0], max?max:min, sizeof(NUMTYPE)*DIMS); + + // copy input data + struct item item; + if (tr->item_clone) { + if (!tr->item_clone(data, (DATATYPE*)&item.data, tr->udata)) { + return false; + } + } else { + memcpy(&item.data, &data, sizeof(DATATYPE)); + } + + while (1) { + if (!tr->root) { + struct node *new_root = node_new(tr, LEAF); + if (!new_root) { + break; + } + tr->root = new_root; + tr->rect = rect; + tr->height = 1; + } + bool split = false; + cow_node_or(tr->root, break); + if (!node_insert(tr, &tr->rect, tr->root, &rect, item, 0, &split)) { + break; + } + if (!split) { + rect_expand(&tr->rect, &rect); + tr->count++; + return true; + } + struct node *new_root = node_new(tr, BRANCH); + if (!new_root) { + break; + } + struct node *right; + if (!node_split(tr, &tr->rect, tr->root, &right)) { + tr->free(new_root); + break; + } + new_root->rects[0] = node_rect_calc(tr->root); + new_root->rects[1] = node_rect_calc(right); + new_root->nodes[0] = tr->root; + new_root->nodes[1] = right; + tr->root = new_root; + tr->root->count = 2; + tr->height++; + } + // out of memory + if (tr->item_free) { + tr->item_free(item.data, tr->udata); + } + return false; +} + +void rtree_destroy(struct rtree *tr) { + if (tr->root) { + node_free(tr, tr->root); + } + tr->free(tr); +} + +static bool node_search(struct node *node, struct rect *rect, + bool (*iter)(const NUMTYPE *min, const NUMTYPE *max, const DATATYPE data, + void *udata), + void *udata) +{ + if (node->kind == LEAF) { + for (int i = 0; i < node->count; i++) { + if (rect_intersects(&node->rects[i], rect)) { + if (!iter(node->rects[i].min, node->rects[i].max, + node->datas[i].data, udata)) + { + return false; + } + } + } + return true; + } + for (int i = 0; i < node->count; i++) { + if (rect_intersects(&node->rects[i], rect)) { + if (!node_search(node->nodes[i], rect, iter, udata)) { + return false; + } + } + } + return true; +} + +void rtree_search(const struct rtree *tr, const NUMTYPE min[], + const NUMTYPE max[], + bool (*iter)(const NUMTYPE min[], const NUMTYPE max[], const DATATYPE data, + void *udata), + void *udata) +{ + // copy input rect + struct rect rect; + memcpy(&rect.min[0], min, sizeof(NUMTYPE)*DIMS); + memcpy(&rect.max[0], max?max:min, sizeof(NUMTYPE)*DIMS); + + if (tr->root) { + node_search(tr->root, &rect, iter, udata); + } +} + +static bool node_scan(struct node *node, + bool (*iter)(const NUMTYPE *min, const NUMTYPE *max, const DATATYPE data, + void *udata), + void *udata) +{ + if (node->kind == LEAF) { + for (int i = 0; i < node->count; i++) { + if (!iter(node->rects[i].min, node->rects[i].max, + node->datas[i].data, udata)) + { + return false; + } + } + return true; + } + for (int i = 0; i < node->count; i++) { + if (!node_scan(node->nodes[i], iter, udata)) { + return false; + } + } + return true; +} + +void rtree_scan(const struct rtree *tr, + bool (*iter)(const NUMTYPE *min, const NUMTYPE *max, const DATATYPE data, + void *udata), + void *udata) +{ + if (tr->root) { + node_scan(tr->root, iter, udata); + } +} + +size_t rtree_count(const struct rtree *tr) { + return tr->count; +} + +static bool node_delete(struct rtree *tr, struct rect *nr, struct node *node, + struct rect *ir, struct item item, int depth, bool *removed, bool *shrunk, + int (*compare)(const DATATYPE a, const DATATYPE b, void *udata), + void *udata) +{ + *removed = false; + *shrunk = false; + if (node->kind == LEAF) { + for (int i = 0; i < node->count; i++) { + if (!rect_equals_bin(ir, &node->rects[i])) { + // Must be exactly the same, binary comparison. + continue; + } + int cmp = compare ? + compare(node->datas[i].data, item.data, udata) : + memcmp(&node->datas[i].data, &item.data, sizeof(DATATYPE)); + if (cmp != 0) { + continue; + } + // Found the target item to delete. + if (tr->item_free) { + tr->item_free(node->datas[i].data, tr->udata); + } + node->rects[i] = node->rects[node->count-1]; + node->datas[i] = node->datas[node->count-1]; + node->count--; + if (rect_onedge(ir, nr)) { + // The item rect was on the edge of the node rect. + // We need to recalculate the node rect. + *nr = node_rect_calc(node); + // Notify the caller that we shrunk the rect. + *shrunk = true; + } + *removed = true; + return true; + } + return true; + } + int h = 0; +#ifdef USE_PATHHINT + h = tr->path_hint[depth]; + if (h < node->count) { + if (rect_contains(&node->rects[h], ir)) { + cow_node_or(node->nodes[h], return false); + if (!node_delete(tr, &node->rects[h], node->nodes[h], ir, item, + depth+1,removed, shrunk, compare, udata)) + { + return false; + } + if (*removed) { + goto removed; + } + } + } + h = 0; +#endif + for (; h < node->count; h++) { + if (!rect_contains(&node->rects[h], ir)) { + continue; + } + struct rect crect = node->rects[h]; + cow_node_or(node->nodes[h], return false); + if (!node_delete(tr, &node->rects[h], node->nodes[h], ir, item, depth+1, + removed, shrunk, compare, udata)) + { + return false; + } + if (!*removed) { + continue; + } + removed: + if (node->nodes[h]->count == 0) { + // underflow + node_free(tr, node->nodes[h]); + node->rects[h] = node->rects[node->count-1]; + node->nodes[h] = node->nodes[node->count-1]; + node->count--; + *nr = node_rect_calc(node); + *shrunk = true; + return true; + } +#ifdef USE_PATHHINT + tr->path_hint[depth] = h; +#endif + if (*shrunk) { + *shrunk = !rect_equals(&node->rects[h], &crect); + if (*shrunk) { + *nr = node_rect_calc(node); + } + } + return true; + } + return true; +} + +// returns false if out of memory +static bool rtree_delete0(struct rtree *tr, const NUMTYPE *min, + const NUMTYPE *max, const DATATYPE data, + int (*compare)(const DATATYPE a, const DATATYPE b, void *udata), + void *udata) +{ + // copy input rect + struct rect rect; + memcpy(&rect.min[0], min, sizeof(NUMTYPE)*DIMS); + memcpy(&rect.max[0], max?max:min, sizeof(NUMTYPE)*DIMS); + + // copy input data + struct item item; + memcpy(&item.data, &data, sizeof(DATATYPE)); + + if (!tr->root) { + return true; + } + bool removed = false; + bool shrunk = false; + cow_node_or(tr->root, return false); + if (!node_delete(tr, &tr->rect, tr->root, &rect, item, 0, &removed, &shrunk, + compare, udata)) + { + return false; + } + if (!removed) { + return true; + } + tr->count--; + if (tr->count == 0) { + node_free(tr, tr->root); + tr->root = NULL; + memset(&tr->rect, 0, sizeof(struct rect)); + tr->height = 0; + } else { + while (tr->root->kind == BRANCH && tr->root->count == 1) { + struct node *prev = tr->root; + tr->root = tr->root->nodes[0]; + prev->count = 0; + node_free(tr, prev); + tr->height--; + } + if (shrunk) { + tr->rect = node_rect_calc(tr->root); + } + } + return true; +} + +bool rtree_delete(struct rtree *tr, const NUMTYPE *min, const NUMTYPE *max, + const DATATYPE data) +{ + return rtree_delete0(tr, min, max, data, NULL, NULL); +} + +bool rtree_delete_with_comparator(struct rtree *tr, const NUMTYPE *min, + const NUMTYPE *max, const DATATYPE data, + int (*compare)(const DATATYPE a, const DATATYPE b, void *udata), + void *udata) +{ + return rtree_delete0(tr, min, max, data, compare, udata); +} + +struct rtree *rtree_clone(struct rtree *tr) { + if (!tr) return NULL; + struct rtree *tr2 = tr->malloc(sizeof(struct rtree)); + if (!tr2) return NULL; + memcpy(tr2, tr, sizeof(struct rtree)); + if (tr2->root) rc_fetch_add(&tr2->root->rc, 1); + return tr2; +} + +void rtree_opt_relaxed_atomics(struct rtree *tr) { + tr->relaxed = true; +} + +#ifdef TEST_PRIVATE_FUNCTIONS +#include "tests/priv_funcs.h" +#endif diff --git a/source/rtree.h b/source/rtree.h new file mode 100644 index 00000000..851ac28b --- /dev/null +++ b/source/rtree.h @@ -0,0 +1,105 @@ +// Copyright 2023 Joshua J Baker. All rights reserved. +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file. + +#ifndef RTREE_H +#define RTREE_H + +#include +#include + +// rtree_new returns a new rtree +// +// Returns NULL if the system is out of memory. +struct rtree *rtree_new(void); + + +// rtree_new returns a new rtree using a custom allocator +// +// Returns NULL if the system is out of memory. +struct rtree *rtree_new_with_allocator(void *(*malloc)(size_t), void (*free)(void*)); + +// rtree_free frees an rtree +void rtree_destroy(struct rtree *tr); + +// rtree_clone makes an instant copy of the btree. +// +// This operation uses shadowing / copy-on-write. +struct rtree *rtree_clone(struct rtree *tr); + +// rtree_set_item_callbacks sets the item clone and free callbacks that will be +// called internally by the rtree when items are inserted and removed. +// +// These callbacks are optional but may be needed by programs that require +// copy-on-write support by using the rtree_clone function. +// +// The clone function should return true if the clone succeeded or false if the +// system is out of memory. +void rtree_set_item_callbacks(struct rtree *tr, + bool (*clone)(const void *item, void **into, void *udata), + void (*free)(const void *item, void *udata)); + +// rtree_set_udata sets the user-defined data. +// +// This should be called once after rtree_new() and is only used for +// the item callbacks as defined in rtree_set_item_callbacks(). +void rtree_set_udata(struct rtree *tr, void *udata); + +// rtree_insert inserts an item into the rtree. +// +// This operation performs a copy of the data that is pointed to in the second +// and third arguments. The R-tree expects a rectangle, which is two arrays of +// doubles. The first N values as the minimum corner of the rect, and the next +// N values as the maximum corner of the rect, where N is the number of +// dimensions. +// +// When inserting points, the max coordinates is optional (set to NULL). +// +// Returns false if the system is out of memory. +bool rtree_insert(struct rtree *tr, const double *min, const double *max, const void *data); + + +// rtree_search searches the rtree and iterates over each item that intersect +// the provided rectangle. +// +// Returning false from the iter will stop the search. +void rtree_search(const struct rtree *tr, const double *min, const double *max, + bool (*iter)(const double *min, const double *max, const void *data, void *udata), + void *udata); + +// rtree_scan iterates over every item in the rtree. +// +// Returning false from the iter will stop the scan. +void rtree_scan(const struct rtree *tr, + bool (*iter)(const double *min, const double *max, const void *data, void *udata), + void *udata); + +// rtree_count returns the number of items in the rtree. +size_t rtree_count(const struct rtree *tr); + +// rtree_delete deletes an item from the rtree. +// +// This searches the tree for an item that is contained within the provided +// rectangle, and perform a binary comparison of its data to the provided +// data. The first item that is found is deleted. +// +// Returns false if the system is out of memory. +bool rtree_delete(struct rtree *tr, const double *min, const double *max, const void *data); + +// rtree_delete_with_comparator deletes an item from the rtree. +// This searches the tree for an item that is contained within the provided +// rectangle, and perform a comparison of its data to the provided data using +// a compare function. The first item that is found is deleted. +// +// Returns false if the system is out of memory. +bool rtree_delete_with_comparator(struct rtree *tr, const double *min, + const double *max, const void *data, + int (*compare)(const void *a, const void *b, void *udata), + void *udata); + +// rtree_opt_relaxed_atomics activates memory_order_relaxed for all atomic +// loads. This may increase performance for single-threaded programs. +// Optionally, define RTREE_NOATOMICS to disbale all atomics. +void rtree_opt_relaxed_atomics(struct rtree *tr); + +#endif // RTREE_H