static uint kdtree2d_balance_recursive( KDTreeNode2D *nodes, uint totnode, axis_t axis, const float (*coords)[2], const uint ofs) { KDTreeNode2D *node; uint neg, pos, median, i, j; if (totnode <= 0) { return KDNODE_UNSET; } else if (totnode == 1) { return 0 + ofs; } /* quicksort style sorting around median */ neg = 0; pos = totnode - 1; median = totnode / 2; while (pos > neg) { const float co = coords[nodes[pos].index][axis]; i = neg - 1; j = pos; while (1) { while (coords[nodes[++i].index][axis] < co) { /* pass */ } while (coords[nodes[--j].index][axis] > co && j > neg) { /* pass */ } if (i >= j) { break; } SWAP(KDTreeNode2D_head, *(KDTreeNode2D_head *)&nodes[i], *(KDTreeNode2D_head *)&nodes[j]); } SWAP(KDTreeNode2D_head, *(KDTreeNode2D_head *)&nodes[i], *(KDTreeNode2D_head *)&nodes[pos]); if (i >= median) { pos = i - 1; } if (i <= median) { neg = i + 1; } } /* set node and sort subnodes */ node = &nodes[median]; node->axis = axis; axis = !axis; node->neg = kdtree2d_balance_recursive(nodes, median, axis, coords, ofs); node->pos = kdtree2d_balance_recursive( &nodes[median + 1], (totnode - (median + 1)), axis, coords, (median + 1) + ofs); return median + ofs; }
static void kdtree2d_balance( struct KDTree2D *tree) { tree->root = kdtree2d_balance_recursive(tree->nodes, tree->totnode, 0, tree->coords, 0); }