Update kd_tree.cpp