Refactoring of kd-tree code