KdTree: Updating Kdtree and PointSET for performance