changed to faster kdtree algorithm (from brute_force)