new_pose2D should be set to the i-th polygon corner inside the for-loop before the if-case. otherwise we always check if the first corner is inside the forbidden area.