Small improve of kd-tree code