Added KDTree optimization for kNN search