Very Simple KDTree + NN Search