KD-Tree build function