Refactoring of kd-tree