Merge KDTree and NDTree improvements