Started on major KDTree overhaul