Very small improvement of kd-tree + refactoring