Assign5: KdTree completed