Update KDTree.cpp