Changeset View
Changeset View
Standalone View
Standalone View
source/blender/blenlib/intern/kdtree_impl.h
- This file was moved from source/blender/blenlib/intern/BLI_kdtree.c.
| Show All 15 Lines | |||||
| /** \file | /** \file | ||||
| * \ingroup bli | * \ingroup bli | ||||
| */ | */ | ||||
| #include "MEM_guardedalloc.h" | #include "MEM_guardedalloc.h" | ||||
| #include "BLI_math.h" | #include "BLI_math.h" | ||||
| #include "BLI_kdtree.h" | #include "BLI_kdtree_impl.h" | ||||
| #include "BLI_utildefines.h" | #include "BLI_utildefines.h" | ||||
| #include "BLI_strict_flags.h" | #include "BLI_strict_flags.h" | ||||
| #define _CONCAT_AUX(MACRO_ARG1, MACRO_ARG2) MACRO_ARG1 ## MACRO_ARG2 | |||||
| #define _CONCAT(MACRO_ARG1, MACRO_ARG2) _CONCAT_AUX(MACRO_ARG1, MACRO_ARG2) | |||||
| #define BLI_KDTREE_PREFIX(id) _CONCAT(KDTREE_PREFIX_ID, _##id) | |||||
| typedef struct KDTreeNode_head { | typedef struct KDTreeNode_head { | ||||
| uint left, right; | uint left, right; | ||||
| float co[3]; | float co[KD_DIMS]; | ||||
| int index; | int index; | ||||
| } KDTreeNode_head; | } KDTreeNode_head; | ||||
| typedef struct KDTreeNode { | typedef struct KDTreeNode { | ||||
| uint left, right; | uint left, right; | ||||
| float co[3]; | float co[KD_DIMS]; | ||||
| int index; | int index; | ||||
| uint d; /* range is only (0-2) */ | uint d; /* range is only (0-KD_DIMS) */ | ||||
intrigus: Shouldn't this be 0-KD_DIMS-1 ?
I.e.: /* range is only (0-KD_DIMS-1) */ | |||||
Done Inline ActionsGood catch, done. campbellbarton: Good catch, done. | |||||
| } KDTreeNode; | } KDTreeNode; | ||||
| struct KDTree { | struct KDTree { | ||||
| KDTreeNode *nodes; | KDTreeNode *nodes; | ||||
| uint nodes_len; | uint nodes_len; | ||||
| uint root; | uint root; | ||||
| #ifdef DEBUG | #ifdef DEBUG | ||||
| bool is_balanced; /* ensure we call balance first */ | bool is_balanced; /* ensure we call balance first */ | ||||
| uint nodes_len_capacity; /* max size of the tree */ | uint nodes_len_capacity; /* max size of the tree */ | ||||
| #endif | #endif | ||||
| }; | }; | ||||
| #define KD_STACK_INIT 100 /* initial size for array (on the stack) */ | #define KD_STACK_INIT 100 /* initial size for array (on the stack) */ | ||||
| #define KD_NEAR_ALLOC_INC 100 /* alloc increment for collecting nearest */ | #define KD_NEAR_ALLOC_INC 100 /* alloc increment for collecting nearest */ | ||||
| #define KD_FOUND_ALLOC_INC 50 /* alloc increment for collecting nearest */ | #define KD_FOUND_ALLOC_INC 50 /* alloc increment for collecting nearest */ | ||||
| #define KD_NODE_UNSET ((uint)-1) | #define KD_NODE_UNSET ((uint)-1) | ||||
| /** When set we know all values are unbalanced, otherwise clear them when re-balancing: see T62210. */ | /** When set we know all values are unbalanced, otherwise clear them when re-balancing: see T62210. */ | ||||
| #define KD_NODE_ROOT_IS_INIT ((uint)-2) | #define KD_NODE_ROOT_IS_INIT ((uint)-2) | ||||
| /* -------------------------------------------------------------------- */ | |||||
| /** \name Local Math API | |||||
| * \{ */ | |||||
| static void copy_vn_vn(float v0[KD_DIMS], const float v1[KD_DIMS]) | |||||
| { | |||||
| for (uint j = 0; j < KD_DIMS; j++) { | |||||
| v0[j] = v1[j]; | |||||
| } | |||||
| } | |||||
| static float len_squared_vnvn(const float v0[KD_DIMS], const float v1[KD_DIMS]) | |||||
| { | |||||
| float d = 0.0f; | |||||
| for (uint j = 0; j < KD_DIMS; j++) { | |||||
| d += SQUARE(v0[j] - v1[j]); | |||||
| } | |||||
| return d; | |||||
| } | |||||
| static float len_squared_vnvn_cb(const float co_kdtree[KD_DIMS], const float co_search[KD_DIMS], const void *UNUSED(user_data)) | |||||
| { | |||||
| return len_squared_vnvn(co_kdtree, co_search); | |||||
| } | |||||
| /** \} */ | |||||
| /** | /** | ||||
| * Creates or free a kdtree | * Creates or free a kdtree | ||||
| */ | */ | ||||
| KDTree *BLI_kdtree_new(uint nodes_len_capacity) | KDTree *BLI_KDTREE_PREFIX(new)(uint nodes_len_capacity) | ||||
| { | { | ||||
| KDTree *tree; | KDTree *tree; | ||||
| tree = MEM_mallocN(sizeof(KDTree), "KDTree"); | tree = MEM_mallocN(sizeof(KDTree), "KDTree"); | ||||
| tree->nodes = MEM_mallocN(sizeof(KDTreeNode) * nodes_len_capacity, "KDTreeNode"); | tree->nodes = MEM_mallocN(sizeof(KDTreeNode) * nodes_len_capacity, "KDTreeNode"); | ||||
| tree->nodes_len = 0; | tree->nodes_len = 0; | ||||
| tree->root = KD_NODE_ROOT_IS_INIT; | tree->root = KD_NODE_ROOT_IS_INIT; | ||||
| #ifdef DEBUG | #ifdef DEBUG | ||||
| tree->is_balanced = false; | tree->is_balanced = false; | ||||
| tree->nodes_len_capacity = nodes_len_capacity; | tree->nodes_len_capacity = nodes_len_capacity; | ||||
| #endif | #endif | ||||
| return tree; | return tree; | ||||
| } | } | ||||
| void BLI_kdtree_free(KDTree *tree) | void BLI_KDTREE_PREFIX(free)(KDTree *tree) | ||||
| { | { | ||||
| if (tree) { | if (tree) { | ||||
| MEM_freeN(tree->nodes); | MEM_freeN(tree->nodes); | ||||
| MEM_freeN(tree); | MEM_freeN(tree); | ||||
| } | } | ||||
| } | } | ||||
| /** | /** | ||||
| * Construction: first insert points, then call balance. Normal is optional. | * Construction: first insert points, then call balance. Normal is optional. | ||||
| */ | */ | ||||
| void BLI_kdtree_insert(KDTree *tree, int index, const float co[3]) | void BLI_KDTREE_PREFIX(insert)(KDTree *tree, int index, const float co[KD_DIMS]) | ||||
| { | { | ||||
| KDTreeNode *node = &tree->nodes[tree->nodes_len++]; | KDTreeNode *node = &tree->nodes[tree->nodes_len++]; | ||||
| #ifdef DEBUG | #ifdef DEBUG | ||||
| BLI_assert(tree->nodes_len <= tree->nodes_len_capacity); | BLI_assert(tree->nodes_len <= tree->nodes_len_capacity); | ||||
| #endif | #endif | ||||
| /* note, array isn't calloc'd, | /* note, array isn't calloc'd, | ||||
| * need to initialize all struct members */ | * need to initialize all struct members */ | ||||
| node->left = node->right = KD_NODE_UNSET; | node->left = node->right = KD_NODE_UNSET; | ||||
| copy_v3_v3(node->co, co); | copy_vn_vn(node->co, co); | ||||
| node->index = index; | node->index = index; | ||||
| node->d = 0; | node->d = 0; | ||||
| #ifdef DEBUG | #ifdef DEBUG | ||||
| tree->is_balanced = false; | tree->is_balanced = false; | ||||
| #endif | #endif | ||||
| } | } | ||||
| Show All 38 Lines | while (right > left) { | ||||
| if (i <= median) { | if (i <= median) { | ||||
| left = i + 1; | left = i + 1; | ||||
| } | } | ||||
| } | } | ||||
| /* set node and sort subnodes */ | /* set node and sort subnodes */ | ||||
| node = &nodes[median]; | node = &nodes[median]; | ||||
| node->d = axis; | node->d = axis; | ||||
| axis = (axis + 1) % 3; | axis = (axis + 1) % KD_DIMS; | ||||
| node->left = kdtree_balance(nodes, median, axis, ofs); | node->left = kdtree_balance(nodes, median, axis, ofs); | ||||
| node->right = kdtree_balance(nodes + median + 1, (nodes_len - (median + 1)), axis, (median + 1) + ofs); | node->right = kdtree_balance(nodes + median + 1, (nodes_len - (median + 1)), axis, (median + 1) + ofs); | ||||
| return median + ofs; | return median + ofs; | ||||
| } | } | ||||
| void BLI_kdtree_balance(KDTree *tree) | void BLI_KDTREE_PREFIX(balance)(KDTree *tree) | ||||
| { | { | ||||
| if (tree->root != KD_NODE_ROOT_IS_INIT) { | if (tree->root != KD_NODE_ROOT_IS_INIT) { | ||||
| for (uint i = 0; i < tree->nodes_len; i++) { | for (uint i = 0; i < tree->nodes_len; i++) { | ||||
| tree->nodes[i].left = KD_NODE_UNSET; | tree->nodes[i].left = KD_NODE_UNSET; | ||||
| tree->nodes[i].right = KD_NODE_UNSET; | tree->nodes[i].right = KD_NODE_UNSET; | ||||
| } | } | ||||
| } | } | ||||
| tree->root = kdtree_balance(tree->nodes, tree->nodes_len, 0, 0); | tree->root = kdtree_balance(tree->nodes, tree->nodes_len, 0, 0); | ||||
| #ifdef DEBUG | #ifdef DEBUG | ||||
| tree->is_balanced = true; | tree->is_balanced = true; | ||||
| #endif | #endif | ||||
| } | } | ||||
| static float len_squared_v3v3_cb(const float co_kdtree[3], const float co_search[3], const void *UNUSED(user_data)) | |||||
| { | |||||
| return len_squared_v3v3(co_kdtree, co_search); | |||||
| } | |||||
| static uint *realloc_nodes(uint *stack, uint *stack_len_capacity, const bool is_alloc) | static uint *realloc_nodes(uint *stack, uint *stack_len_capacity, const bool is_alloc) | ||||
| { | { | ||||
| uint *stack_new = MEM_mallocN((*stack_len_capacity + KD_NEAR_ALLOC_INC) * sizeof(uint), "KDTree.treestack"); | uint *stack_new = MEM_mallocN((*stack_len_capacity + KD_NEAR_ALLOC_INC) * sizeof(uint), "KDTree.treestack"); | ||||
| memcpy(stack_new, stack, *stack_len_capacity * sizeof(uint)); | memcpy(stack_new, stack, *stack_len_capacity * sizeof(uint)); | ||||
| // memset(stack_new + *stack_len_capacity, 0, sizeof(uint) * KD_NEAR_ALLOC_INC); | // memset(stack_new + *stack_len_capacity, 0, sizeof(uint) * KD_NEAR_ALLOC_INC); | ||||
| if (is_alloc) { | if (is_alloc) { | ||||
| MEM_freeN(stack); | MEM_freeN(stack); | ||||
| } | } | ||||
| *stack_len_capacity += KD_NEAR_ALLOC_INC; | *stack_len_capacity += KD_NEAR_ALLOC_INC; | ||||
| return stack_new; | return stack_new; | ||||
| } | } | ||||
| /** | /** | ||||
| * Find nearest returns index, and -1 if no node is found. | * Find nearest returns index, and -1 if no node is found. | ||||
| */ | */ | ||||
| int BLI_kdtree_find_nearest( | int BLI_KDTREE_PREFIX(find_nearest)( | ||||
| const KDTree *tree, const float co[3], | const KDTree *tree, const float co[KD_DIMS], | ||||
| KDTreeNearest *r_nearest) | KDTreeNearest *r_nearest) | ||||
| { | { | ||||
| const KDTreeNode *nodes = tree->nodes; | const KDTreeNode *nodes = tree->nodes; | ||||
| const KDTreeNode *root, *min_node; | const KDTreeNode *root, *min_node; | ||||
| uint *stack, stack_default[KD_STACK_INIT]; | uint *stack, stack_default[KD_STACK_INIT]; | ||||
| float min_dist, cur_dist; | float min_dist, cur_dist; | ||||
| uint stack_len_capacity, cur = 0; | uint stack_len_capacity, cur = 0; | ||||
| #ifdef DEBUG | #ifdef DEBUG | ||||
| BLI_assert(tree->is_balanced == true); | BLI_assert(tree->is_balanced == true); | ||||
| #endif | #endif | ||||
| if (UNLIKELY(tree->root == KD_NODE_UNSET)) { | if (UNLIKELY(tree->root == KD_NODE_UNSET)) { | ||||
| return -1; | return -1; | ||||
| } | } | ||||
| stack = stack_default; | stack = stack_default; | ||||
| stack_len_capacity = KD_STACK_INIT; | stack_len_capacity = KD_STACK_INIT; | ||||
| root = &nodes[tree->root]; | root = &nodes[tree->root]; | ||||
| min_node = root; | min_node = root; | ||||
| min_dist = len_squared_v3v3(root->co, co); | min_dist = len_squared_vnvn(root->co, co); | ||||
| if (co[root->d] < root->co[root->d]) { | if (co[root->d] < root->co[root->d]) { | ||||
| if (root->right != KD_NODE_UNSET) { | if (root->right != KD_NODE_UNSET) { | ||||
| stack[cur++] = root->right; | stack[cur++] = root->right; | ||||
| } | } | ||||
| if (root->left != KD_NODE_UNSET) { | if (root->left != KD_NODE_UNSET) { | ||||
| stack[cur++] = root->left; | stack[cur++] = root->left; | ||||
| } | } | ||||
| Show All 11 Lines | while (cur--) { | ||||
| const KDTreeNode *node = &nodes[stack[cur]]; | const KDTreeNode *node = &nodes[stack[cur]]; | ||||
| cur_dist = node->co[node->d] - co[node->d]; | cur_dist = node->co[node->d] - co[node->d]; | ||||
| if (cur_dist < 0.0f) { | if (cur_dist < 0.0f) { | ||||
| cur_dist = -cur_dist * cur_dist; | cur_dist = -cur_dist * cur_dist; | ||||
| if (-cur_dist < min_dist) { | if (-cur_dist < min_dist) { | ||||
| cur_dist = len_squared_v3v3(node->co, co); | cur_dist = len_squared_vnvn(node->co, co); | ||||
| if (cur_dist < min_dist) { | if (cur_dist < min_dist) { | ||||
| min_dist = cur_dist; | min_dist = cur_dist; | ||||
| min_node = node; | min_node = node; | ||||
| } | } | ||||
| if (node->left != KD_NODE_UNSET) { | if (node->left != KD_NODE_UNSET) { | ||||
| stack[cur++] = node->left; | stack[cur++] = node->left; | ||||
| } | } | ||||
| } | } | ||||
| if (node->right != KD_NODE_UNSET) { | if (node->right != KD_NODE_UNSET) { | ||||
| stack[cur++] = node->right; | stack[cur++] = node->right; | ||||
| } | } | ||||
| } | } | ||||
| else { | else { | ||||
| cur_dist = cur_dist * cur_dist; | cur_dist = cur_dist * cur_dist; | ||||
| if (cur_dist < min_dist) { | if (cur_dist < min_dist) { | ||||
| cur_dist = len_squared_v3v3(node->co, co); | cur_dist = len_squared_vnvn(node->co, co); | ||||
| if (cur_dist < min_dist) { | if (cur_dist < min_dist) { | ||||
| min_dist = cur_dist; | min_dist = cur_dist; | ||||
| min_node = node; | min_node = node; | ||||
| } | } | ||||
| if (node->right != KD_NODE_UNSET) { | if (node->right != KD_NODE_UNSET) { | ||||
| stack[cur++] = node->right; | stack[cur++] = node->right; | ||||
| } | } | ||||
| } | } | ||||
| if (node->left != KD_NODE_UNSET) { | if (node->left != KD_NODE_UNSET) { | ||||
| stack[cur++] = node->left; | stack[cur++] = node->left; | ||||
| } | } | ||||
| } | } | ||||
| if (UNLIKELY(cur + 3 > stack_len_capacity)) { | if (UNLIKELY(cur + KD_DIMS > stack_len_capacity)) { | ||||
| stack = realloc_nodes(stack, &stack_len_capacity, stack_default != stack); | stack = realloc_nodes(stack, &stack_len_capacity, stack_default != stack); | ||||
| } | } | ||||
| } | } | ||||
| if (r_nearest) { | if (r_nearest) { | ||||
| r_nearest->index = min_node->index; | r_nearest->index = min_node->index; | ||||
| r_nearest->dist = sqrtf(min_dist); | r_nearest->dist = sqrtf(min_dist); | ||||
| copy_v3_v3(r_nearest->co, min_node->co); | copy_vn_vn(r_nearest->co, min_node->co); | ||||
| } | } | ||||
| if (stack != stack_default) { | if (stack != stack_default) { | ||||
| MEM_freeN(stack); | MEM_freeN(stack); | ||||
| } | } | ||||
| return min_node->index; | return min_node->index; | ||||
| } | } | ||||
| /** | /** | ||||
| * A version of #BLI_kdtree_find_nearest which runs a callback | * A version of #BLI_kdtree_find_nearest which runs a callback | ||||
| * to filter out values. | * to filter out values. | ||||
| * | * | ||||
| * \param filter_cb: Filter find results, | * \param filter_cb: Filter find results, | ||||
| * Return codes: (1: accept, 0: skip, -1: immediate exit). | * Return codes: (1: accept, 0: skip, -1: immediate exit). | ||||
| */ | */ | ||||
| int BLI_kdtree_find_nearest_cb( | int BLI_KDTREE_PREFIX(find_nearest_cb)( | ||||
| const KDTree *tree, const float co[3], | const KDTree *tree, const float co[KD_DIMS], | ||||
| int (*filter_cb)(void *user_data, int index, const float co[3], float dist_sq), void *user_data, | int (*filter_cb)(void *user_data, int index, const float co[KD_DIMS], float dist_sq), void *user_data, | ||||
| KDTreeNearest *r_nearest) | KDTreeNearest *r_nearest) | ||||
| { | { | ||||
| const KDTreeNode *nodes = tree->nodes; | const KDTreeNode *nodes = tree->nodes; | ||||
| const KDTreeNode *min_node = NULL; | const KDTreeNode *min_node = NULL; | ||||
| uint *stack, stack_default[KD_STACK_INIT]; | uint *stack, stack_default[KD_STACK_INIT]; | ||||
| float min_dist = FLT_MAX, cur_dist; | float min_dist = FLT_MAX, cur_dist; | ||||
| uint stack_len_capacity, cur = 0; | uint stack_len_capacity, cur = 0; | ||||
| #ifdef DEBUG | #ifdef DEBUG | ||||
| BLI_assert(tree->is_balanced == true); | BLI_assert(tree->is_balanced == true); | ||||
| #endif | #endif | ||||
| if (UNLIKELY(tree->root == KD_NODE_UNSET)) { | if (UNLIKELY(tree->root == KD_NODE_UNSET)) { | ||||
| return -1; | return -1; | ||||
| } | } | ||||
| stack = stack_default; | stack = stack_default; | ||||
| stack_len_capacity = ARRAY_SIZE(stack_default); | stack_len_capacity = ARRAY_SIZE(stack_default); | ||||
| #define NODE_TEST_NEAREST(node) \ | #define NODE_TEST_NEAREST(node) \ | ||||
| { \ | { \ | ||||
| const float dist_sq = len_squared_v3v3((node)->co, co); \ | const float dist_sq = len_squared_vnvn((node)->co, co); \ | ||||
| if (dist_sq < min_dist) { \ | if (dist_sq < min_dist) { \ | ||||
| const int result = filter_cb(user_data, (node)->index, (node)->co, dist_sq); \ | const int result = filter_cb(user_data, (node)->index, (node)->co, dist_sq); \ | ||||
| if (result == 1) { \ | if (result == 1) { \ | ||||
| min_dist = dist_sq; \ | min_dist = dist_sq; \ | ||||
| min_node = node; \ | min_node = node; \ | ||||
| } \ | } \ | ||||
| else if (result == 0) { \ | else if (result == 0) { \ | ||||
| /* pass */ \ | /* pass */ \ | ||||
| Show All 35 Lines | else { | ||||
| if (node->right != KD_NODE_UNSET) { | if (node->right != KD_NODE_UNSET) { | ||||
| stack[cur++] = node->right; | stack[cur++] = node->right; | ||||
| } | } | ||||
| } | } | ||||
| if (node->left != KD_NODE_UNSET) { | if (node->left != KD_NODE_UNSET) { | ||||
| stack[cur++] = node->left; | stack[cur++] = node->left; | ||||
| } | } | ||||
| } | } | ||||
| if (UNLIKELY(cur + 3 > stack_len_capacity)) { | if (UNLIKELY(cur + KD_DIMS > stack_len_capacity)) { | ||||
| stack = realloc_nodes(stack, &stack_len_capacity, stack_default != stack); | stack = realloc_nodes(stack, &stack_len_capacity, stack_default != stack); | ||||
| } | } | ||||
| } | } | ||||
| #undef NODE_TEST_NEAREST | #undef NODE_TEST_NEAREST | ||||
| finally: | finally: | ||||
| if (stack != stack_default) { | if (stack != stack_default) { | ||||
| MEM_freeN(stack); | MEM_freeN(stack); | ||||
| } | } | ||||
| if (min_node) { | if (min_node) { | ||||
| if (r_nearest) { | if (r_nearest) { | ||||
| r_nearest->index = min_node->index; | r_nearest->index = min_node->index; | ||||
| r_nearest->dist = sqrtf(min_dist); | r_nearest->dist = sqrtf(min_dist); | ||||
| copy_v3_v3(r_nearest->co, min_node->co); | copy_vn_vn(r_nearest->co, min_node->co); | ||||
| } | } | ||||
| return min_node->index; | return min_node->index; | ||||
| } | } | ||||
| else { | else { | ||||
| return -1; | return -1; | ||||
| } | } | ||||
| } | } | ||||
| static void nearest_ordered_insert( | static void nearest_ordered_insert( | ||||
| KDTreeNearest *nearest, uint *nearest_len, const uint nearest_len_capacity, | KDTreeNearest *nearest, uint *nearest_len, const uint nearest_len_capacity, | ||||
| const int index, const float dist, const float co[3]) | const int index, const float dist, const float co[KD_DIMS]) | ||||
| { | { | ||||
| uint i; | uint i; | ||||
| if (*nearest_len < nearest_len_capacity) { | if (*nearest_len < nearest_len_capacity) { | ||||
| (*nearest_len)++; | (*nearest_len)++; | ||||
| } | } | ||||
| for (i = *nearest_len - 1; i > 0; i--) { | for (i = *nearest_len - 1; i > 0; i--) { | ||||
| if (dist >= nearest[i - 1].dist) { | if (dist >= nearest[i - 1].dist) { | ||||
| break; | break; | ||||
| } | } | ||||
| else { | else { | ||||
| nearest[i] = nearest[i - 1]; | nearest[i] = nearest[i - 1]; | ||||
| } | } | ||||
| } | } | ||||
| nearest[i].index = index; | nearest[i].index = index; | ||||
| nearest[i].dist = dist; | nearest[i].dist = dist; | ||||
| copy_v3_v3(nearest[i].co, co); | copy_vn_vn(nearest[i].co, co); | ||||
| } | } | ||||
| /** | /** | ||||
| * Find \a nearest_len_capacity nearest returns number of points found, with results in nearest. | * Find \a nearest_len_capacity nearest returns number of points found, with results in nearest. | ||||
| * | * | ||||
| * \param r_nearest: An array of nearest, sized at least \a nearest_len_capacity. | * \param r_nearest: An array of nearest, sized at least \a nearest_len_capacity. | ||||
| */ | */ | ||||
| int BLI_kdtree_find_nearest_n_with_len_squared_cb( | int BLI_KDTREE_PREFIX(find_nearest_n_with_len_squared_cb)( | ||||
| const KDTree *tree, const float co[3], | const KDTree *tree, const float co[KD_DIMS], | ||||
| KDTreeNearest r_nearest[], | KDTreeNearest r_nearest[], | ||||
| const uint nearest_len_capacity, | const uint nearest_len_capacity, | ||||
| float (*len_sq_fn)(const float co_search[3], const float co_test[3], const void *user_data), | float (*len_sq_fn)(const float co_search[KD_DIMS], const float co_test[KD_DIMS], const void *user_data), | ||||
| const void *user_data) | const void *user_data) | ||||
| { | { | ||||
| const KDTreeNode *nodes = tree->nodes; | const KDTreeNode *nodes = tree->nodes; | ||||
| const KDTreeNode *root; | const KDTreeNode *root; | ||||
| uint *stack, stack_default[KD_STACK_INIT]; | uint *stack, stack_default[KD_STACK_INIT]; | ||||
| float cur_dist; | float cur_dist; | ||||
| uint stack_len_capacity, cur = 0; | uint stack_len_capacity, cur = 0; | ||||
| uint i, nearest_len = 0; | uint i, nearest_len = 0; | ||||
| #ifdef DEBUG | #ifdef DEBUG | ||||
| BLI_assert(tree->is_balanced == true); | BLI_assert(tree->is_balanced == true); | ||||
| #endif | #endif | ||||
| if (UNLIKELY((tree->root == KD_NODE_UNSET) || nearest_len_capacity == 0)) { | if (UNLIKELY((tree->root == KD_NODE_UNSET) || nearest_len_capacity == 0)) { | ||||
| return 0; | return 0; | ||||
| } | } | ||||
| if (len_sq_fn == NULL) { | if (len_sq_fn == NULL) { | ||||
| len_sq_fn = len_squared_v3v3_cb; | len_sq_fn = len_squared_vnvn_cb; | ||||
| BLI_assert(user_data == NULL); | BLI_assert(user_data == NULL); | ||||
| } | } | ||||
| stack = stack_default; | stack = stack_default; | ||||
| stack_len_capacity = ARRAY_SIZE(stack_default); | stack_len_capacity = ARRAY_SIZE(stack_default); | ||||
| root = &nodes[tree->root]; | root = &nodes[tree->root]; | ||||
| ▲ Show 20 Lines • Show All 52 Lines • ▼ Show 20 Lines | else { | ||||
| if (node->right != KD_NODE_UNSET) { | if (node->right != KD_NODE_UNSET) { | ||||
| stack[cur++] = node->right; | stack[cur++] = node->right; | ||||
| } | } | ||||
| } | } | ||||
| if (node->left != KD_NODE_UNSET) { | if (node->left != KD_NODE_UNSET) { | ||||
| stack[cur++] = node->left; | stack[cur++] = node->left; | ||||
| } | } | ||||
| } | } | ||||
| if (UNLIKELY(cur + 3 > stack_len_capacity)) { | if (UNLIKELY(cur + KD_DIMS > stack_len_capacity)) { | ||||
| stack = realloc_nodes(stack, &stack_len_capacity, stack_default != stack); | stack = realloc_nodes(stack, &stack_len_capacity, stack_default != stack); | ||||
| } | } | ||||
| } | } | ||||
| for (i = 0; i < nearest_len; i++) { | for (i = 0; i < nearest_len; i++) { | ||||
| r_nearest[i].dist = sqrtf(r_nearest[i].dist); | r_nearest[i].dist = sqrtf(r_nearest[i].dist); | ||||
| } | } | ||||
| if (stack != stack_default) { | if (stack != stack_default) { | ||||
| MEM_freeN(stack); | MEM_freeN(stack); | ||||
| } | } | ||||
| return (int)nearest_len; | return (int)nearest_len; | ||||
| } | } | ||||
| int BLI_KDTREE_PREFIX(find_nearest_n)( | |||||
| const KDTree *tree, const float co[KD_DIMS], | |||||
| KDTreeNearest r_nearest[], | |||||
| const uint nearest_len_capacity) | |||||
| { | |||||
| return BLI_KDTREE_PREFIX(find_nearest_n_with_len_squared_cb)( | |||||
| tree, co, r_nearest, nearest_len_capacity, | |||||
| NULL, NULL); | |||||
| } | |||||
| static int nearest_cmp_dist(const void *a, const void *b) | static int nearest_cmp_dist(const void *a, const void *b) | ||||
| { | { | ||||
| const KDTreeNearest *kda = a; | const KDTreeNearest *kda = a; | ||||
| const KDTreeNearest *kdb = b; | const KDTreeNearest *kdb = b; | ||||
| if (kda->dist < kdb->dist) { | if (kda->dist < kdb->dist) { | ||||
| return -1; | return -1; | ||||
| } | } | ||||
| else if (kda->dist > kdb->dist) { | else if (kda->dist > kdb->dist) { | ||||
| return 1; | return 1; | ||||
| } | } | ||||
| else { | else { | ||||
| return 0; | return 0; | ||||
| } | } | ||||
| } | } | ||||
| static void nearest_add_in_range( | static void nearest_add_in_range( | ||||
| KDTreeNearest **r_nearest, | KDTreeNearest **r_nearest, | ||||
| uint nearest_index, | uint nearest_index, | ||||
| uint *nearest_len_capacity, | uint *nearest_len_capacity, | ||||
| const int index, const float dist, const float co[3]) | const int index, const float dist, const float co[KD_DIMS]) | ||||
| { | { | ||||
| KDTreeNearest *to; | KDTreeNearest *to; | ||||
| if (UNLIKELY(nearest_index >= *nearest_len_capacity)) { | if (UNLIKELY(nearest_index >= *nearest_len_capacity)) { | ||||
| *r_nearest = MEM_reallocN_id( | *r_nearest = MEM_reallocN_id( | ||||
| *r_nearest, | *r_nearest, | ||||
| (*nearest_len_capacity += KD_FOUND_ALLOC_INC) * sizeof(KDTreeNode), | (*nearest_len_capacity += KD_FOUND_ALLOC_INC) * sizeof(KDTreeNode), | ||||
| __func__); | __func__); | ||||
| } | } | ||||
| to = (*r_nearest) + nearest_index; | to = (*r_nearest) + nearest_index; | ||||
| to->index = index; | to->index = index; | ||||
| to->dist = sqrtf(dist); | to->dist = sqrtf(dist); | ||||
| copy_v3_v3(to->co, co); | copy_vn_vn(to->co, co); | ||||
| } | } | ||||
| /** | /** | ||||
| * Range search returns number of points nearest_len, with results in nearest | * Range search returns number of points nearest_len, with results in nearest | ||||
| * | * | ||||
| * \param r_nearest: Allocated array of nearest nearest_len (caller is responsible for freeing). | * \param r_nearest: Allocated array of nearest nearest_len (caller is responsible for freeing). | ||||
| */ | */ | ||||
| int BLI_kdtree_range_search_with_len_squared_cb( | int BLI_KDTREE_PREFIX(range_search_with_len_squared_cb)( | ||||
| const KDTree *tree, const float co[3], | const KDTree *tree, const float co[KD_DIMS], | ||||
| KDTreeNearest **r_nearest, const float range, | KDTreeNearest **r_nearest, const float range, | ||||
| float (*len_sq_fn)(const float co_search[3], const float co_test[3], const void *user_data), | float (*len_sq_fn)(const float co_search[KD_DIMS], const float co_test[KD_DIMS], const void *user_data), | ||||
| const void *user_data) | const void *user_data) | ||||
| { | { | ||||
| const KDTreeNode *nodes = tree->nodes; | const KDTreeNode *nodes = tree->nodes; | ||||
| uint *stack, stack_default[KD_STACK_INIT]; | uint *stack, stack_default[KD_STACK_INIT]; | ||||
| KDTreeNearest *nearest = NULL; | KDTreeNearest *nearest = NULL; | ||||
| const float range_sq = range * range; | const float range_sq = range * range; | ||||
| float dist_sq; | float dist_sq; | ||||
| uint stack_len_capacity, cur = 0; | uint stack_len_capacity, cur = 0; | ||||
| uint nearest_len = 0, nearest_len_capacity = 0; | uint nearest_len = 0, nearest_len_capacity = 0; | ||||
| #ifdef DEBUG | #ifdef DEBUG | ||||
| BLI_assert(tree->is_balanced == true); | BLI_assert(tree->is_balanced == true); | ||||
| #endif | #endif | ||||
| if (UNLIKELY(tree->root == KD_NODE_UNSET)) { | if (UNLIKELY(tree->root == KD_NODE_UNSET)) { | ||||
| return 0; | return 0; | ||||
| } | } | ||||
| if (len_sq_fn == NULL) { | if (len_sq_fn == NULL) { | ||||
| len_sq_fn = len_squared_v3v3_cb; | len_sq_fn = len_squared_vnvn_cb; | ||||
| BLI_assert(user_data == NULL); | BLI_assert(user_data == NULL); | ||||
| } | } | ||||
| stack = stack_default; | stack = stack_default; | ||||
| stack_len_capacity = ARRAY_SIZE(stack_default); | stack_len_capacity = ARRAY_SIZE(stack_default); | ||||
| stack[cur++] = tree->root; | stack[cur++] = tree->root; | ||||
| Show All 19 Lines | else { | ||||
| if (node->left != KD_NODE_UNSET) { | if (node->left != KD_NODE_UNSET) { | ||||
| stack[cur++] = node->left; | stack[cur++] = node->left; | ||||
| } | } | ||||
| if (node->right != KD_NODE_UNSET) { | if (node->right != KD_NODE_UNSET) { | ||||
| stack[cur++] = node->right; | stack[cur++] = node->right; | ||||
| } | } | ||||
| } | } | ||||
| if (UNLIKELY(cur + 3 > stack_len_capacity)) { | if (UNLIKELY(cur + KD_DIMS > stack_len_capacity)) { | ||||
| stack = realloc_nodes(stack, &stack_len_capacity, stack_default != stack); | stack = realloc_nodes(stack, &stack_len_capacity, stack_default != stack); | ||||
| } | } | ||||
| } | } | ||||
| if (stack != stack_default) { | if (stack != stack_default) { | ||||
| MEM_freeN(stack); | MEM_freeN(stack); | ||||
| } | } | ||||
| if (nearest_len) { | if (nearest_len) { | ||||
| qsort(nearest, nearest_len, sizeof(KDTreeNearest), nearest_cmp_dist); | qsort(nearest, nearest_len, sizeof(KDTreeNearest), nearest_cmp_dist); | ||||
| } | } | ||||
| *r_nearest = nearest; | *r_nearest = nearest; | ||||
| return (int)nearest_len; | return (int)nearest_len; | ||||
| } | } | ||||
| int BLI_KDTREE_PREFIX(range_search)( | |||||
| const KDTree *tree, const float co[KD_DIMS], | |||||
| KDTreeNearest **r_nearest, const float range) | |||||
| { | |||||
| return BLI_KDTREE_PREFIX(range_search_with_len_squared_cb)( | |||||
| tree, co, r_nearest, range, | |||||
| NULL, NULL); | |||||
| } | |||||
| /** | /** | ||||
| * A version of #BLI_kdtree_range_search which runs a callback | * A version of #BLI_kdtree_range_search which runs a callback | ||||
| * instead of allocating an array. | * instead of allocating an array. | ||||
| * | * | ||||
| * \param search_cb: Called for every node found in \a range, false return value performs an early exit. | * \param search_cb: Called for every node found in \a range, false return value performs an early exit. | ||||
| * | * | ||||
| * \note the order of calls isn't sorted based on distance. | * \note the order of calls isn't sorted based on distance. | ||||
| */ | */ | ||||
| void BLI_kdtree_range_search_cb( | void BLI_KDTREE_PREFIX(range_search_cb)( | ||||
| const KDTree *tree, const float co[3], float range, | const KDTree *tree, const float co[KD_DIMS], float range, | ||||
| bool (*search_cb)(void *user_data, int index, const float co[3], float dist_sq), void *user_data) | bool (*search_cb)(void *user_data, int index, const float co[KD_DIMS], float dist_sq), void *user_data) | ||||
| { | { | ||||
| const KDTreeNode *nodes = tree->nodes; | const KDTreeNode *nodes = tree->nodes; | ||||
| uint *stack, stack_default[KD_STACK_INIT]; | uint *stack, stack_default[KD_STACK_INIT]; | ||||
| float range_sq = range * range, dist_sq; | float range_sq = range * range, dist_sq; | ||||
| uint stack_len_capacity, cur = 0; | uint stack_len_capacity, cur = 0; | ||||
| #ifdef DEBUG | #ifdef DEBUG | ||||
| Show All 18 Lines | if (co[node->d] + range < node->co[node->d]) { | ||||
| } | } | ||||
| } | } | ||||
| else if (co[node->d] - range > node->co[node->d]) { | else if (co[node->d] - range > node->co[node->d]) { | ||||
| if (node->right != KD_NODE_UNSET) { | if (node->right != KD_NODE_UNSET) { | ||||
| stack[cur++] = node->right; | stack[cur++] = node->right; | ||||
| } | } | ||||
| } | } | ||||
| else { | else { | ||||
| dist_sq = len_squared_v3v3(node->co, co); | dist_sq = len_squared_vnvn(node->co, co); | ||||
| if (dist_sq <= range_sq) { | if (dist_sq <= range_sq) { | ||||
| if (search_cb(user_data, node->index, node->co, dist_sq) == false) { | if (search_cb(user_data, node->index, node->co, dist_sq) == false) { | ||||
| goto finally; | goto finally; | ||||
| } | } | ||||
| } | } | ||||
| if (node->left != KD_NODE_UNSET) { | if (node->left != KD_NODE_UNSET) { | ||||
| stack[cur++] = node->left; | stack[cur++] = node->left; | ||||
| } | } | ||||
| if (node->right != KD_NODE_UNSET) { | if (node->right != KD_NODE_UNSET) { | ||||
| stack[cur++] = node->right; | stack[cur++] = node->right; | ||||
| } | } | ||||
| } | } | ||||
| if (UNLIKELY(cur + 3 > stack_len_capacity)) { | if (UNLIKELY(cur + KD_DIMS > stack_len_capacity)) { | ||||
| stack = realloc_nodes(stack, &stack_len_capacity, stack_default != stack); | stack = realloc_nodes(stack, &stack_len_capacity, stack_default != stack); | ||||
| } | } | ||||
| } | } | ||||
| finally: | finally: | ||||
| if (stack != stack_default) { | if (stack != stack_default) { | ||||
| MEM_freeN(stack); | MEM_freeN(stack); | ||||
| } | } | ||||
| Show All 21 Lines | struct DeDuplicateParams { | ||||
| /* Static */ | /* Static */ | ||||
| const KDTreeNode *nodes; | const KDTreeNode *nodes; | ||||
| float range; | float range; | ||||
| float range_sq; | float range_sq; | ||||
| int *duplicates; | int *duplicates; | ||||
| int *duplicates_found; | int *duplicates_found; | ||||
| /* Per Search */ | /* Per Search */ | ||||
| float search_co[3]; | float search_co[KD_DIMS]; | ||||
| int search; | int search; | ||||
| }; | }; | ||||
| static void deduplicate_recursive(const struct DeDuplicateParams *p, uint i) | static void deduplicate_recursive(const struct DeDuplicateParams *p, uint i) | ||||
| { | { | ||||
| const KDTreeNode *node = &p->nodes[i]; | const KDTreeNode *node = &p->nodes[i]; | ||||
| if (p->search_co[node->d] + p->range <= node->co[node->d]) { | if (p->search_co[node->d] + p->range <= node->co[node->d]) { | ||||
| if (node->left != KD_NODE_UNSET) { | if (node->left != KD_NODE_UNSET) { | ||||
| deduplicate_recursive(p, node->left); | deduplicate_recursive(p, node->left); | ||||
| } | } | ||||
| } | } | ||||
| else if (p->search_co[node->d] - p->range >= node->co[node->d]) { | else if (p->search_co[node->d] - p->range >= node->co[node->d]) { | ||||
| if (node->right != KD_NODE_UNSET) { | if (node->right != KD_NODE_UNSET) { | ||||
| deduplicate_recursive(p, node->right); | deduplicate_recursive(p, node->right); | ||||
| } | } | ||||
| } | } | ||||
| else { | else { | ||||
| if ((p->search != node->index) && (p->duplicates[node->index] == -1)) { | if ((p->search != node->index) && (p->duplicates[node->index] == -1)) { | ||||
| if (compare_len_squared_v3v3(node->co, p->search_co, p->range_sq)) { | if (len_squared_vnvn(node->co, p->search_co) <= p->range_sq) { | ||||
| p->duplicates[node->index] = (int)p->search; | p->duplicates[node->index] = (int)p->search; | ||||
| *p->duplicates_found += 1; | *p->duplicates_found += 1; | ||||
| } | } | ||||
| } | } | ||||
| if (node->left != KD_NODE_UNSET) { | if (node->left != KD_NODE_UNSET) { | ||||
| deduplicate_recursive(p, node->left); | deduplicate_recursive(p, node->left); | ||||
| } | } | ||||
| if (node->right != KD_NODE_UNSET) { | if (node->right != KD_NODE_UNSET) { | ||||
| Show All 15 Lines | |||||
| * \param duplicates: An array of int's the length of #KDTree.nodes_len | * \param duplicates: An array of int's the length of #KDTree.nodes_len | ||||
| * Values initialized to -1 are candidates to me merged. | * Values initialized to -1 are candidates to me merged. | ||||
| * Setting the index to it's own position in the array prevents it from being touched, | * Setting the index to it's own position in the array prevents it from being touched, | ||||
| * although it can still be used as a target. | * although it can still be used as a target. | ||||
| * \returns The number of merges found (includes any merges already in the \a duplicates array). | * \returns The number of merges found (includes any merges already in the \a duplicates array). | ||||
| * | * | ||||
| * \note Merging is always a single step (target indices wont be marked for merging). | * \note Merging is always a single step (target indices wont be marked for merging). | ||||
| */ | */ | ||||
| int BLI_kdtree_calc_duplicates_fast( | int BLI_KDTREE_PREFIX(calc_duplicates_fast)( | ||||
| const KDTree *tree, const float range, bool use_index_order, | const KDTree *tree, const float range, bool use_index_order, | ||||
| int *duplicates) | int *duplicates) | ||||
| { | { | ||||
| int found = 0; | int found = 0; | ||||
| struct DeDuplicateParams p = { | struct DeDuplicateParams p = { | ||||
| .nodes = tree->nodes, | .nodes = tree->nodes, | ||||
| .range = range, | .range = range, | ||||
| .range_sq = SQUARE(range), | .range_sq = SQUARE(range), | ||||
| .duplicates = duplicates, | .duplicates = duplicates, | ||||
| .duplicates_found = &found, | .duplicates_found = &found, | ||||
| }; | }; | ||||
| if (use_index_order) { | if (use_index_order) { | ||||
| uint *order = kdtree_order(tree); | uint *order = kdtree_order(tree); | ||||
| for (uint i = 0; i < tree->nodes_len; i++) { | for (uint i = 0; i < tree->nodes_len; i++) { | ||||
| const uint node_index = order[i]; | const uint node_index = order[i]; | ||||
| const int index = (int)i; | const int index = (int)i; | ||||
| if (ELEM(duplicates[index], -1, index)) { | if (ELEM(duplicates[index], -1, index)) { | ||||
| p.search = index; | p.search = index; | ||||
| copy_v3_v3(p.search_co, tree->nodes[node_index].co); | copy_vn_vn(p.search_co, tree->nodes[node_index].co); | ||||
| int found_prev = found; | int found_prev = found; | ||||
| deduplicate_recursive(&p, tree->root); | deduplicate_recursive(&p, tree->root); | ||||
| if (found != found_prev) { | if (found != found_prev) { | ||||
| /* Prevent chains of doubles. */ | /* Prevent chains of doubles. */ | ||||
| duplicates[index] = index; | duplicates[index] = index; | ||||
| } | } | ||||
| } | } | ||||
| } | } | ||||
| MEM_freeN(order); | MEM_freeN(order); | ||||
| } | } | ||||
| else { | else { | ||||
| for (uint i = 0; i < tree->nodes_len; i++) { | for (uint i = 0; i < tree->nodes_len; i++) { | ||||
| const uint node_index = i; | const uint node_index = i; | ||||
| const int index = p.nodes[node_index].index; | const int index = p.nodes[node_index].index; | ||||
| if (ELEM(duplicates[index], -1, index)) { | if (ELEM(duplicates[index], -1, index)) { | ||||
| p.search = index; | p.search = index; | ||||
| copy_v3_v3(p.search_co, tree->nodes[node_index].co); | copy_vn_vn(p.search_co, tree->nodes[node_index].co); | ||||
| int found_prev = found; | int found_prev = found; | ||||
| deduplicate_recursive(&p, tree->root); | deduplicate_recursive(&p, tree->root); | ||||
| if (found != found_prev) { | if (found != found_prev) { | ||||
| /* Prevent chains of doubles. */ | /* Prevent chains of doubles. */ | ||||
| duplicates[index] = index; | duplicates[index] = index; | ||||
| } | } | ||||
| } | } | ||||
| } | } | ||||
| } | } | ||||
| return found; | return found; | ||||
| } | } | ||||
| /** \} */ | /** \} */ | ||||
Shouldn't this be 0-KD_DIMS-1 ?
I.e.: /* range is only (0-KD_DIMS-1) */