Update KDTree code