kdtree building reimplemented