Slight modifications to kd_tree