Update kdtree2.cpp