static uint kdtree_balance(KDTreeNode *nodes, uint totnode, uint axis, const uint ofs)
{
KDTreeNode *node;
float co;
uint left, right, median, i, j;
if (totnode <= 0)
return KD_NODE_UNSET;
else if (totnode == 1)
return 0 + ofs;
/* quicksort style sorting around median */
left = 0;
right = totnode - 1;
median = totnode / 2;
while (right > left) {
co = nodes[right].co[axis];
i = left - 1;
j = right;
while (1) {
while (nodes[++i].co[axis] < co) ;
while (nodes[--j].co[axis] > co && j > left) ;
if (i >= j)
break;
SWAP(KDTreeNode_head, *(KDTreeNode_head *)&nodes[i], *(KDTreeNode_head *)&nodes[j]);
}
SWAP(KDTreeNode_head, *(KDTreeNode_head *)&nodes[i], *(KDTreeNode_head *)&nodes[right]);
if (i >= median)
right = i - 1;
if (i <= median)
left = i + 1;
}
/* set node and sort subnodes */
node = &nodes[median];
node->d = axis;
axis = (axis + 1) % 3;
node->left = kdtree_balance(nodes, median, axis, ofs);
node->right = kdtree_balance(nodes + median + 1, (totnode - (median + 1)), axis, (median + 1) + ofs);
return median + ofs;
}
void BLI_kdtree_balance(KDTree *tree)
{
tree->root = kdtree_balance(tree->nodes, tree->totnode, 0);
#ifdef DEBUG
tree->is_balanced = true;
#endif
}
static KDTreeNode *kdtree_balance(KDTreeNode *nodes, unsigned int totnode, unsigned int axis)
{
KDTreeNode *node;
float co;
unsigned int left, right, median, i, j;
if (totnode <= 0)
return NULL;
else if (totnode == 1)
return nodes;
/* quicksort style sorting around median */
left = 0;
right = totnode - 1;
median = totnode / 2;
while (right > left) {
co = nodes[right].co[axis];
i = left - 1;
j = right;
while (1) {
while (nodes[++i].co[axis] < co) ;
while (nodes[--j].co[axis] > co && j > left) ;
if (i >= j)
break;
SWAP(KDTreeNode, nodes[i], nodes[j]);
}
SWAP(KDTreeNode, nodes[i], nodes[right]);
if (i >= median)
right = i - 1;
if (i <= median)
left = i + 1;
}
/* set node and sort subnodes */
node = &nodes[median];
node->d = axis;
node->left = kdtree_balance(nodes, median, (axis + 1) % 3);
node->right = kdtree_balance(nodes + median + 1, (totnode - (median + 1)), (axis + 1) % 3);
return node;
}
void BLI_kdtree_balance(KDTree *tree)
{
tree->root = kdtree_balance(tree->nodes, tree->totnode, 0);
}