Added KdTree helpers