pcf kdtree, closest point, refactor