Solve kd-tree bug