Working kd-tree pre optimization