Simplify KD tree code