KDTree optimizations