diff options
Diffstat (limited to 'src/ix2.c')
| -rw-r--r-- | src/ix2.c | 632 |
1 files changed, 632 insertions, 0 deletions
diff --git a/src/ix2.c b/src/ix2.c new file mode 100644 index 0000000..fcdeb79 --- /dev/null +++ b/src/ix2.c @@ -0,0 +1,632 @@ +/* + * Copyright (C) 2018 Vito Caputo - <vcaputo@pengaru.com> + * + * This program is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License version 3 as published + * by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + */ + +/* 2d spatial index - implemented as a quadtree */ + +/* There are various outstanding TODO items littered throughout this code, + * it's usable as-is but by no means finished or optimized. + */ + +#include <assert.h> +#include <stdlib.h> + +#include "ix2.h" +#include "list.h" + +typedef struct v2f_t { + float x, y; +} v2f_t; + +typedef struct aabb_t { + v2f_t min, max; +} aabb_t; + +typedef struct ix2_node_t ix2_node_t; + +typedef struct ix2_object_t { + list_head_t references; /* list of all ix2_node references to this object */ + list_head_t hits; /* node on hit list for non-point searches, to prevent potential double hits like when an object and search both span nodes */ + /* XXX: note this makes non-point searches non-reentrant! i.e. you can't perform another non-point search on the same ix2 + * from a search callback operating on the same ix2. TODO investigate efficient solutions. An obvious one is to simply put + * multiple of these nodes in every object and support a small number of simultaneous non-point searches, which may suffice. + */ + aabb_t aabb; /* AABB of this object */ + float aabb_len_sq; /* length(aabb.max - aabb.min)^2 TODO: this isn't utilized yet, optimization potential */ + void *object; /* user handle linking this object to the index */ +} ix2_object_t; + +typedef struct ix2_object_ref_t { + list_head_t objects; /* node on ix2_node objects list */ + list_head_t references; /* node on ix2_object references list */ + ix2_object_t *object; /* object being referenced */ + ix2_node_t *node; /* node referencing object */ +} ix2_object_ref_t; + +struct ix2_node_t { + v2f_t center; /* spatial center point of ix2_node */ + ix2_node_t *children; /* array of 4 child quadrants when not a leaf, NULL when a leaf */ + unsigned n_objects; /* number of objects in leaf when !children */ + list_head_t objects; /* list of all objects in leaf when !children */ +}; + +typedef struct ix2_t { + aabb_t aabb; /* spatial bounds of ix2 root */ + unsigned max_per_node; /* maximum number of objects to put in a node until max_depth reached */ + unsigned max_depth; /* maximum depth of ix2 tree where nodes cease being divided further */ + ix2_node_t root; /* root node of ix2 quadtree */ +} ix2_t; + +static ix2_object_t * add_object(ix2_t *ix2, unsigned *depth, ix2_node_t *node, aabb_t *node_aabb, ix2_object_t *object, ix2_object_ref_t **reference_cache); + + +/* Check if point resides within aabb */ +static inline int aabb_contains_point(aabb_t *aabb, v2f_t *point) +{ + assert(aabb); + assert(point); + + if (point->x < aabb->min.x) + return 0; + + if (point->x > aabb->max.x) + return 0; + + if (point->y < aabb->min.y) + return 0; + + if (point->y > aabb->max.y) + return 0; + + return 1; +} + + +/* check if a and b overlap */ +static inline int aabb_overlap(aabb_t *a, aabb_t *b) +{ + assert(a); + assert(b); + + return (a->min.x <= b->max.x && + a->max.x >= b->min.x && + a->min.y <= b->max.y && + a->max.y >= b->min.y); +} + + +/* initialize a node centered in aabb */ +static void init_node(ix2_node_t *node, aabb_t *aabb) +{ + assert(node); + assert(aabb); + + INIT_LIST_HEAD(&node->objects); + + node->center.x = aabb->min.x; + node->center.y = aabb->min.y; + node->center.x += .5f * (aabb->max.x - aabb->min.x); + node->center.y += .5f * (aabb->max.y - aabb->min.y); +} + + +/* initialize a quadrants aabb_t array from a parent node aabb and center point */ +static void init_quadrants(aabb_t *quadrants, aabb_t *parent_aabb, v2f_t *quadrants_center) +{ + assert(quadrants); + assert(parent_aabb); + assert(quadrants_center); +/* + +-+-+ + |0|1| + +-+-+ + |2|3| + +-+-+ +*/ + quadrants[0].min.x = parent_aabb->min.x; + quadrants[0].min.y = quadrants_center->y; + quadrants[0].max.x = quadrants_center->x; + quadrants[0].max.y = parent_aabb->max.y; + + quadrants[1].min.x = quadrants_center->x; + quadrants[1].min.y = quadrants_center->y; + quadrants[1].max.x = parent_aabb->max.x; + quadrants[1].max.y = parent_aabb->max.y; + + quadrants[2].min.x = parent_aabb->min.x; + quadrants[2].min.y = parent_aabb->min.y; + quadrants[2].max.x = quadrants_center->x; + quadrants[2].max.y = quadrants_center->y; + + quadrants[3].min.x = quadrants_center->x; + quadrants[3].min.y = parent_aabb->min.y; + quadrants[3].max.x = parent_aabb->max.x; + quadrants[3].max.y = quadrants_center->y; +} + + +/* Unlink a single object reference, disconnecting it from the object and node */ +static void unlink_object_ref(ix2_object_ref_t *ref) +{ + assert(ref); + + /* TODO: is there an actual need to maintain and detect the unlinked state w/NULL ptrs? */ + if (!ref->node) + return; + + list_del(&ref->objects); + list_del(&ref->references); + ref->node->n_objects--; + ref->object = NULL; + ref->node = NULL; +} + + +/* Unlink an object from the index, unlinking all node references. + * If references_cache is NULL, all unlinked references are freed. + * Otherwise, the unlinked references are added to the references_cache list. + */ +static void unlink_object(ix2_object_t *object, list_head_t *references_cache) +{ + ix2_object_ref_t *ref, *_ref; + + assert(object); + + list_for_each_entry_safe(ref, _ref, &object->references, references) { + unlink_object_ref(ref); + + if (references_cache) { + list_add_tail(&ref->references, references_cache); + continue; + } + + free(ref); + } +} + + +/* Link an object into a node. + * If ref is not NULL, it's an existing reference to move to a new node rather than allocating a new one. + * Returns the reference on success, NULL on failure + * On falure the supplied object is left intact. + */ +static ix2_object_ref_t * link_object(ix2_object_t *object, ix2_node_t *node, ix2_object_ref_t **cached_ref) +{ + ix2_object_ref_t *ref = NULL; + + assert(object); + assert(node); + + if (cached_ref && *cached_ref) { + ref = *cached_ref; + *cached_ref = NULL; + unlink_object_ref(ref); + } + + if (!ref) + ref = calloc(1, sizeof(ix2_object_ref_t)); + + if (!ref) + return NULL; + + list_add_tail(&ref->references, &object->references); + list_add_tail(&ref->objects, &node->objects); + ref->object = object; + ref->node = node; + node->n_objects++; + + return ref; +} + + +/* Compute the diagonal length of the aabb, leaving it squared */ +static inline float aabb_len_sq(aabb_t *aabb) +{ + float len_sq; + + assert(aabb); + + /* TODO: import the full v2f.h and do this with v2f_sub() && v2f_dot()? */ + len_sq = (aabb->max.x - aabb->min.x) * (aabb->max.x - aabb->min.x); + len_sq += (aabb->max.y - aabb->min.y) * (aabb->max.y - aabb->min.y); + + return len_sq; +} + + +/* remove an object */ +static void remove_object(ix2_object_t *object) +{ + assert(object); + + unlink_object(object, NULL); + free(object); +} + + +/* Split node and distribute objects referenced from node->objects into a + * newly created node->children. + */ +ix2_node_t * split_node(ix2_t *ix2, unsigned *depth, ix2_node_t *node, aabb_t *node_aabb) +{ + aabb_t quadrants[4]; + ix2_object_ref_t *r, *_r; + int i; + + assert(ix2); + assert(depth); + assert(node); + assert(!node->children); + assert(node_aabb); + + /* allocate and setup the children nodes */ + node->children = calloc(4, sizeof(ix2_node_t)); + if (!node->children) + goto _fail; + + init_quadrants(quadrants, node_aabb, &node->center); + for (i = 0; i < 4; i++) + init_node(&node->children[i], &quadrants[i]); + + /* distribute objects across node->children, removing node->objects + * references along the way. The references are supplied + */ + list_for_each_entry_safe(r, _r, &node->objects, objects) { + ix2_object_t *o = r->object; + + /* XXX: this can probably be done more efficiently */ + for (i = 0; i < 4; i++) { + if (!aabb_overlap(&quadrants[i], &o->aabb)) + continue; + + if (!add_object(ix2, depth, &node->children[i], &quadrants[i], o, &r)) + goto _fail; + } + } + + return node->children; + +_fail: + /* TODO: some kind of cleanup? */ + return NULL; +} + + +/* Add a member object to the space described by node && node_aabb + * + * This function may need to divide nodes and/or recur. + * + * Since it may need to allocate memory for object references via + * link_object(), and may need to allocate space for children + * nodes, it may fail. + * + * As an optimization, a list of object refs may be supplied in object_refs for + * use when linking the object. + * + * On failure NULL is returned and object is freed (including all its references). + * On success the supplied object is simply returned, with potentialy new references added. + */ +static ix2_object_t * add_object(ix2_t *ix2, unsigned *depth, ix2_node_t *node, aabb_t *node_aabb, ix2_object_t *object, ix2_object_ref_t **reference_cache) +{ + assert(ix2); + assert(depth); + assert(node); + assert(node_aabb); + assert(object); + + /* If not a leaf, simply recur on overlapping children */ + if (node->children) { + aabb_t quadrants[4]; + int i; + + *depth++; + init_quadrants(quadrants, node_aabb, &node->center); + for (i = 0; i < 4; i++) { + if (!aabb_overlap(&quadrants[i], &object->aabb)) + continue; + + if (!add_object(ix2, depth, &node->children[i], &quadrants[i], object, reference_cache)) + goto _fail; + } + *depth--; + + return object; + } + + + /* Node is a leaf, optimistically link the object to the node */ + if (!link_object(object, node, reference_cache)) + goto _fail; + + + /* If the node is overflowing, split it */ + if (node->n_objects > ix2->max_per_node && *depth < ix2->max_depth) { + *depth++; + if (!split_node(ix2, depth, node, node_aabb)) + goto _fail; + *depth--; + } + + return object; + +_fail: + remove_object(object); + + return NULL; +} + + +/* Create a new ix2 index with bounds aabb, splitting nodes > max_per_node until max_depth */ +ix2_t * ix2_new(aabb_t *aabb, unsigned max_per_node, unsigned max_depth) +{ + ix2_t *ix2; + + assert(aabb); + + ix2 = calloc(1, sizeof(ix2_t)); + if (!ix2) + return NULL; + + init_node(&ix2->root, aabb); + + ix2->aabb = *aabb; + ix2->max_per_node = max_per_node; + ix2->max_depth = max_depth; + + return ix2; +} + + +/* Free the index ix2 and everything associated with it */ +/* Note the external objects which have been indexed are not freed */ +void ix2_free(ix2_t *ix2) +{ + /* TODO: free everything */ + free(ix2); +} + + +/* Insert object spatially described by object_aabb into the index ix2 */ +ix2_object_t * ix2_insert_object(ix2_t *ix2, aabb_t *object_aabb, void *object) +{ + unsigned depth = 0; + ix2_object_t *o; + + assert(ix2); + assert(object_aabb); + assert(object_aabb->min.x <= object_aabb->max.x); + assert(object_aabb->min.y <= object_aabb->max.y); + + o = calloc(1, sizeof(ix2_object_t)); + if (!o) + return NULL; + + INIT_LIST_HEAD(&o->references); + INIT_LIST_HEAD(&o->hits); + + o->aabb = *object_aabb; + o->object = object; + o->aabb_len_sq = aabb_len_sq(object_aabb); + + if (!aabb_overlap(&ix2->aabb, object_aabb)) + return object; + + return add_object(ix2, &depth, &ix2->root, &ix2->aabb, o, NULL); +} + + +/* Remove object from the index ix2 and free it. */ +/* This may be either the object handle returned by ix2_node_insert(), or + * the handle supplied to ix2_search_cb(). + */ +void ix2_remove_object(ix2_t *ix2, ix2_object_t *object) +{ + assert(ix2); + assert(object); + + remove_object(object); +} + + +/* Move already inserted object to new aabb. */ +/* Returns object on success, NULL on failure. */ +/* On failure object is removed and freed. XXX: change to leave where it was? */ +ix2_object_t * ix2_move_object(ix2_t *ix2, ix2_object_t *object, aabb_t *object_aabb) +{ + unsigned depth = 0; + + assert(ix2); + assert(object); + assert(object_aabb); + assert(object_aabb->min.x <= object_aabb->max.x); + assert(object_aabb->min.y <= object_aabb->max.y); + + /* TODO FIXME: cache and reuse the references, requires changing + * add_object to receive a list of cached references rather than the + * singleton it currently expects. + * unlink_object() already knows how to leave those references on a + * supplied list head which is currently NULL. + */ + unlink_object(object, NULL); + + object->aabb = *object_aabb; + object->aabb_len_sq = aabb_len_sq(object_aabb); + + if (!aabb_overlap(&ix2->aabb, object_aabb)) + return object; + + return add_object(ix2, &depth, &ix2->root, &ix2->aabb, object, NULL /* TODO */); +} + + +/* Search for objects overlapping a point in index ix2. */ +/* Returns number of cb calls performed on hits, if cb is NULL calls will be skipped but hits still counted for return. */ +unsigned ix2_search_by_point(ix2_t *ix2, v2f_t *point, ix2_search_cb cb, void *cb_context) +{ + unsigned n_hits = 0; + ix2_object_ref_t *ref, *_ref; + ix2_node_t *node; + aabb_t node_aabb; + + assert(ix2); + assert(point); + + if (!aabb_contains_point(&ix2->aabb, point)) + return 0; + + node = &ix2->root; + node_aabb = ix2->aabb; + + /* find the leaf node containing point */ + while (node->children) { + aabb_t quadrants[4]; + int i; + + /* TODO: this can be done more efficiently - + * by simply comparing point to the center and + * deriving the index from that + */ + init_quadrants(quadrants, &node_aabb, &node->center); + for (i = 0; i < 4; i++) { + if (aabb_contains_point(&quadrants[i], point)) + break; + } + + node = &node->children[i]; + node_aabb = quadrants[i]; + } + + /* search the leaf for matches */ + list_for_each_entry_safe(ref, _ref, &node->objects, objects) { + if (!aabb_contains_point(&ref->object->aabb, point)) + continue; + + if (cb) { + ix2_search_status_t r; + + r = cb(cb_context, ref->object, &ref->object->aabb, ref->object->object); + if (r == IX2_SEARCH_STOP) + break; + + if (r == IX2_SEARCH_IGNORE) + continue; + } + + n_hits++; + } + + + return n_hits; +} + + +/* recursively search the specified node for objects overlapping with search_aabb, + * add hits to the list @ hits. + */ +static void search_node_by_aabb(ix2_node_t *node, aabb_t *node_aabb, aabb_t *search_aabb, list_head_t *hits) +{ + ix2_object_ref_t *ref, *_ref; + + assert(node); + assert(node_aabb); + assert(search_aabb); + assert(hits); + + if (node->children) { + aabb_t quadrants[4]; + int i; + + init_quadrants(quadrants, node_aabb, &node->center); + for (i = 0; i < 4; i++) { + if (aabb_overlap(&quadrants[i], search_aabb)) + search_node_by_aabb(&node->children[i], &quadrants[i], search_aabb, hits); + } + + return; + } + + + list_for_each_entry_safe(ref, _ref, &node->objects, objects) { + if (!aabb_overlap(&ref->object->aabb, search_aabb)) + continue; + + list_move_tail(&ref->object->hits, hits); + } +} + + +/* Search for objects overlapping an aabb in index ix2. */ +/* Returns number of cb calls performed on hits, if cb is NULL calls will be skipped but hits still counted for return. */ +unsigned ix2_search_by_aabb(ix2_t *ix2, aabb_t *search_aabb, ix2_search_cb cb, void *cb_context) +{ + unsigned n_hits = 0; + LIST_HEAD (hits); + ix2_object_t *o, *_o; + + assert(ix2); + assert(search_aabb); + + if (!aabb_overlap(&ix2->aabb, search_aabb)) + return 0; + + /* XXX: for the aabb-based search a list of hits is first constructed, + * this prevents multiple callbacks on the same object by first + * allowing those multiple hits to simply move the object within the + * hits list. In a separate pass the hits are processed with the + * callbacks. + * + * There are two unfortunate consquences to this approach: + * 1. No nested/reentrant aabb searches on the same ix2, as the hit list + * node is embedded in the ix2_object_t. + * 2. No way to terminate the search from the callback. + * This may be mitigated by adding a limit count to the search + * function, but that can't address scenarios where you want to + * terminate the search on something arbitrary like after the first + * hit of a specific object type. + * + * TODO: investigate alternative solutions. Obvious options: + * - Putting more than one hit list node in every object for limited + * nesting. + * - Maintain a per-search hash-table which gets accessed before + * every callback to see if the object has been seen already, adding + * it if not before calling the callback. This strikes me as very + * heavyweight. + */ + search_node_by_aabb(&ix2->root, &ix2->aabb, search_aabb, &hits); + + list_for_each_entry_safe(o, _o, &hits, hits) { + list_del_init(&o->hits); + + if (cb) { + ix2_search_status_t r; + + r = cb(cb_context, o, &o->aabb, o->object); + if (r == IX2_SEARCH_STOP) + break; + + if (r == IX2_SEARCH_IGNORE) + continue; + } + + n_hits++; + } + + /* just in case the search was aborted, finish deleting these nodes */ + /* FIXME: this whole hit list thing is pretty janky */ + list_for_each_entry_safe(o, _o, &hits, hits) + list_del_init(&o->hits); + + return n_hits; +} |