Going with a more object oriented KD tree