Update KDTree code, not tested yet
Revert "Update KDTree code, not tested yet"

This reverts commit e85abe383a645396d3c01a890107521474484864.