summaryrefslogtreecommitdiffstats
path: root/kd-forest.c
diff options
context:
space:
mode:
Diffstat (limited to 'kd-forest.c')
-rw-r--r--kd-forest.c327
1 files changed, 0 insertions, 327 deletions
diff --git a/kd-forest.c b/kd-forest.c
deleted file mode 100644
index 3840a61..0000000
--- a/kd-forest.c
+++ /dev/null
@@ -1,327 +0,0 @@
-/*********************************************************************
- * kd-forest *
- * Copyright (C) 2014 Tavian Barnes <tavianator@tavianator.com> *
- * *
- * This program is free software. It comes without any warranty, to *
- * the extent permitted by applicable law. You can redistribute it *
- * and/or modify it under the terms of the Do What The Fuck You Want *
- * To Public License, Version 2, as published by Sam Hocevar. See *
- * the COPYING file or http://www.wtfpl.net/ for more details. *
- *********************************************************************/
-
-#include "kd-forest.h"
-#include "util.h"
-#include <math.h>
-#include <stdbool.h>
-#include <stdlib.h>
-#include <string.h>
-
-kd_node_t *
-new_kd_node(double coords[KD_DIMEN], unsigned int x, unsigned int y)
-{
- kd_node_t *node = xmalloc(sizeof(kd_node_t));
- memcpy(node->coords, coords, sizeof(node->coords));
- node->left = node->right = NULL;
- node->x = x;
- node->y = y;
- node->removed = false;
- return node;
-}
-
-static void
-kd_destroy(kd_node_t *node)
-{
- if (node) {
- kd_destroy(node->left);
- kd_destroy(node->right);
- free(node);
- }
-}
-
-static size_t
-kd_collect_nodes(kd_node_t *root, kd_node_t **buffer, bool include_removed)
-{
- size_t count = 0;
-
- if (include_removed || !root->removed) {
- buffer[0] = root;
- ++count;
- }
-
- if (root->left) {
- count += kd_collect_nodes(root->left, buffer + count, include_removed);
- }
- if (root->right) {
- count += kd_collect_nodes(root->right, buffer + count, include_removed);
- }
-
- if (root->removed && !include_removed) {
- free(root);
- }
-
- return count;
-}
-
-typedef int kd_comparator(const void *a, const void* b);
-
-static int kd_compare0(const void *a, const void *b) {
- double aval = (*(const kd_node_t **)a)->coords[0];
- double bval = (*(const kd_node_t **)b)->coords[0];
- return (aval > bval) - (aval < bval);
-}
-
-static int kd_compare1(const void *a, const void *b) {
- double aval = (*(const kd_node_t **)a)->coords[1];
- double bval = (*(const kd_node_t **)b)->coords[1];
- return (aval > bval) - (aval < bval);
-}
-
-static int kd_compare2(const void *a, const void *b) {
- double aval = (*(const kd_node_t **)a)->coords[2];
- double bval = (*(const kd_node_t **)b)->coords[2];
- return (aval > bval) - (aval < bval);
-}
-
-static kd_comparator *kd_comparators[KD_DIMEN] = {
- kd_compare0,
- kd_compare1,
- kd_compare2,
-};
-
-// When building k-d trees, we use KD_DIMEN sorted arrays of nodes plus one
-// extra array for scratch space
-#define KD_BUFSIZE (KD_DIMEN + 1)
-
-static kd_node_t *
-kd_build_tree_recursive(kd_node_t **buffers[KD_BUFSIZE], size_t size, unsigned int coord)
-{
- if (size == 0) {
- return NULL;
- }
-
- size_t split = size/2;
- size_t left_size = split, right_size = size - left_size - 1;
- kd_node_t *root = buffers[coord][split];
- for (size_t i = 0; i < size; ++i) {
- buffers[coord][i]->is_left = i < left_size;
- }
-
- kd_node_t **right_buffers[KD_BUFSIZE];
- for (int i = 0; i < KD_DIMEN; ++i) {
- right_buffers[i] = buffers[i] + left_size + 1;
- }
-
- kd_node_t **scratch = buffers[KD_DIMEN];
- right_buffers[KD_DIMEN] = scratch;
-
- for (size_t i = 0; i < KD_DIMEN; ++i) {
- if (i == coord) {
- continue;
- }
-
- kd_node_t **buffer = buffers[i];
- kd_node_t **right_buffer = right_buffers[i];
- for (size_t j = 0, k = 0, skip = 0; j < size; ++j) {
- if (buffer[j]->is_left) {
- buffer[j - skip] = buffer[j];
- } else {
- if (buffer[j] != root) {
- scratch[k] = buffer[j];
- ++k;
- }
- ++skip;
- }
- }
- for (size_t j = 0; j < right_size; ++j) {
- right_buffer[j] = scratch[j];
- }
- }
-
- coord = (coord + 1)%KD_DIMEN;
- root->left = kd_build_tree_recursive(buffers, left_size, coord);
- root->right = kd_build_tree_recursive(right_buffers, right_size, coord);
-
- return root;
-}
-
-static kd_node_t *
-kd_build_tree(kd_node_t **buffers[KD_BUFSIZE], size_t size)
-{
- for (int i = 1; i < KD_DIMEN; ++i) {
- memcpy(buffers[i], buffers[0], size*sizeof(kd_node_t *));
- }
- for (int i = 0; i < KD_DIMEN; ++i) {
- qsort(buffers[i], size, sizeof(kd_node_t *), kd_comparators[i]);
- }
- return kd_build_tree_recursive(buffers, size, 0);
-}
-
-static double
-kd_distance_sq(const double a[KD_DIMEN], const double b[KD_DIMEN])
-{
- double result = 0.0;
- for (int i = 0; i < KD_DIMEN; ++i) {
- double d = a[i] - b[i];
- result += d*d;
- }
- return result;
-}
-
-static void
-kd_find_nearest_recursive(kd_node_t *node, const double target[KD_DIMEN], const double closest[KD_DIMEN], kd_node_t **best, double *limit, unsigned int coord)
-{
- if (!node->removed) {
- double node_dist_sq = kd_distance_sq(node->coords, target);
- if (node_dist_sq < *limit) {
- *limit = node_dist_sq;
- *best = node;
- }
- }
-
- kd_node_t *first;
- kd_node_t *second;
- if (target[coord] < node->coords[coord]) {
- first = node->left;
- second = node->right;
- } else {
- first = node->right;
- second = node->left;
- }
-
- unsigned int next = (coord + 1)%KD_DIMEN;
-
- if (first) {
- kd_find_nearest_recursive(first, target, closest, best, limit, next);
- }
-
- if (second) {
- double new_closest[KD_DIMEN];
- memcpy(new_closest, closest, sizeof(new_closest));
- new_closest[coord] = node->coords[coord];
-
- if (kd_distance_sq(new_closest, target) < *limit) {
- kd_find_nearest_recursive(second, target, new_closest, best, limit, next);
- }
- }
-}
-
-static void
-kd_find_nearest(kd_node_t *root, const double target[KD_DIMEN], kd_node_t **best, double *limit)
-{
- kd_find_nearest_recursive(root, target, target, best, limit, 0);
-}
-
-void
-kdf_init(kd_forest_t *kdf)
-{
- kdf->roots = NULL;
- kdf->size = kdf->size_est = 0;
- kdf->roots_size = kdf->roots_capacity = 0;
-}
-
-void
-kdf_destroy(kd_forest_t *kdf)
-{
- for (unsigned int i = 0; i < kdf->roots_size; ++i) {
- kd_destroy(kdf->roots[i]);
- }
-
- free(kdf->roots);
-}
-
-static size_t
-kdf_collect_nodes(kd_forest_t *kdf, kd_node_t **buffer, unsigned int slot, bool include_removed)
-{
- size_t count = 0;
- for (unsigned int i = 0; i < slot; ++i) {
- if (kdf->roots[i]) {
- count += kd_collect_nodes(kdf->roots[i], buffer + count, include_removed);
- }
- }
- return count;
-}
-
-static void
-kdf_balance(kd_forest_t *kdf, kd_node_t *new_node, bool force)
-{
- ++kdf->size;
-
- size_t slot, buffer_size;
- if (force) {
- buffer_size = kdf->size_est = kdf->size;
- slot = kdf->roots_size;
- } else {
- ++kdf->size_est;
- for (slot = 0; slot < kdf->roots_size; ++slot) {
- if (!kdf->roots[slot]) {
- break;
- }
- }
- buffer_size = 1 << slot;
- }
-
- kd_node_t **buffer = xmalloc(buffer_size*sizeof(kd_node_t *));
- buffer[0] = new_node;
- kdf_collect_nodes(kdf, buffer + 1, slot, !force);
-
- kd_node_t **buffers[KD_BUFSIZE];
- for (int i = 1; i < KD_BUFSIZE; ++i) {
- buffers[i] = xmalloc(buffer_size*sizeof(kd_node_t *));
- }
-
- if (slot >= kdf->roots_capacity) {
- kdf->roots_capacity = slot + 1;
- kdf->roots = xrealloc(kdf->roots, kdf->roots_capacity*sizeof(kd_node_t *));
- }
-
- size_t i, offset;
- for (i = 0, offset = 0; offset < buffer_size; ++i) {
- size_t chunk_size = 1 << i;
- if (buffer_size & chunk_size) {
- buffers[0] = buffer + offset;
- kdf->roots[i] = kd_build_tree(buffers, chunk_size);
- offset |= chunk_size;
- } else {
- kdf->roots[i] = NULL;
- }
- }
- if (force || i > kdf->roots_size) {
- kdf->roots_size = i;
- }
-
- free(buffer);
- for (i = 1; i < KD_BUFSIZE; ++i) {
- free(buffers[i]);
- }
-}
-
-void
-kdf_insert(kd_forest_t *kdf, kd_node_t *node)
-{
- // If half or more of the nodes are removed, force a complete rebalance
- bool force = (kdf->size_est + 1) >= 2*(kdf->size + 1);
- kdf_balance(kdf, node, force);
-}
-
-void
-kdf_remove(kd_forest_t *kdf, kd_node_t *node)
-{
- --kdf->size;
- node->removed = true;
-}
-
-kd_node_t *
-kdf_find_nearest(const kd_forest_t *kdf, const double target[KD_DIMEN])
-{
- double limit = INFINITY;
- kd_node_t *best = NULL;
-
- for (unsigned int i = 0; i < kdf->roots_size; ++i) {
- kd_node_t *root = kdf->roots[i];
- if (root != NULL) {
- kd_find_nearest(root, target, &best, &limit);
- }
- }
-
- return best;
-}
generated by cgit v1.2.3 (git 2.39.1) at 2024-05-02 04:42:34 +0000