题目描述

机械臂逆解模块可能给出多个候选关节姿态。具身智能部需要从这些候选中选出一个既满足末端目标误差、又不与矩形障碍物相交的姿态。

对每个候选 (q1, q2)，先检查关节范围，再用二连杆正运动学计算肘点和末端。两段线段 (0,0) 到肘点、肘点到末端都不能与任何矩形障碍物相交或接触。通过碰撞检查后，若末端位置误差不超过 tol_pos 且朝向误差不超过 tol_ang，该候选可用。

在所有可用候选中，依次按位置误差、朝向误差、关节改变量 |q1-cur1|+|q2-cur2|、编号升序选择最优。

输入格式

第一行输入 L1 L2 tool。

第二行输入关节范围 q1min q1max q2min q2max。

第三行输入当前关节角 cur1 cur2。

第四行输入候选数量 M。

接下来 M 行，每行输入 id q1 q2。

下一行输入目标末端位姿 xg yg alpha。

下一行输入误差阈值 tol_pos tol_ang。

下一行输入矩形障碍数量 B。

接下来 B 行，每行输入一个矩形 x1 y1 x2 y2。

输出格式

若存在可用候选，输出：

id x y pos_error

其中 x y 是该候选的末端坐标，pos_error 是位置误差，均保留 6 位小数。

若没有可用候选，输出 NO_POSE。

判定规则

使用标准输出比较。角度误差按归一化到 (-180, 180] 后的绝对差计算。线段与矩形边界接触也视为碰撞。

数据范围与测试提示

• 正式测试会覆盖样例没有出现的边界输入，请不要依赖样例规模或样例顺序写特判。
• 输出只包含题目要求的字段，不要输出调试信息；浮点结果按题面要求保留 6 位小数。
• 候选姿态可能全部被碰撞、角度或距离筛掉，此时要按无解分支输出。
• 角度误差需要归一化到 (-180, 180] 后取绝对值；线段刚好接触矩形边界也视为碰撞。
• 无可用姿态输出 NO_POSE；可用姿态并列时按位置误差、角度误差、关节变化、编号排序。

样例

输入样例 1

2 1 0
-180 180 -180 180
0 0
3
1 0 0
2 0 90
3 90 -90
3 0 0
2 20
0

输出样例 1

1 3.000000 0.000000 0.000000

输入样例 2

2 1 0
-90 90 -90 90
0 0
1
1 0 90
0 3 90
0.1 5
0

输出样例 2

NO_POSE