Draft implementation of building KDTree