Differential D4536 Diff 14233 source/blender/blenlib/intern/kdtree_impl.h

Changeset 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) */
		intrigusUnsubmitted Done Inline Actions Shouldn't this be 0-KD_DIMS-1 ? I.e.: /* range is only (0-KD_DIMS-1) / intrigus:* Shouldn't this be 0-KD_DIMS-1 ? I.e.: /* range is only (0-KD_DIMS-1) */
		campbellbartonAuthorUnsubmitted Done Inline Actions Good 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)

		static void copy_vn_vn(float v0[KD_DIMS], const float v1[KD_DIMS]);

/**		/**
* 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))		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 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_cb(const float co_kdtree[KD_DIMS], const float co_search[KD_DIMS], const void *UNUSED(user_data))
{		{
return len_squared_v3v3(co_kdtree, co_search);		return len_squared_vnvn(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;
}		}

/** \} */		/** \} */