#AEAI04002. 多级坐标链中的目标定位

多级坐标链中的目标定位

题目描述

具身智能部的识别结果会依次经过相机、末端执行器和机械臂基座等多个二维坐标系。现在给出 3 级刚体变换,以及若干候选目标在初始坐标系中的位置。请依次应用这 3 个变换,把每个候选目标转换到最终坐标系,并选出距离给定目标点最近的候选。

每一级变换都由平移 (tx, ty) 和逆时针旋转角 theta 表示,作用公式与二维刚体变换一致。若多个候选到目标点的距离相同,选择编号更小的候选。

输入格式

前 3 行,每行输入一个变换 tx ty theta,按输入顺序依次作用。

第四行输入整数 N

接下来 N 行,每行输入一个候选点的初始坐标 x y,候选编号按输入顺序从 1 开始。

最后一行输入目标点坐标 gx gy

输出格式

输出一行:被选中的候选编号,以及该候选转换后的最终坐标 X Y。坐标保留 6 位小数。

判定规则

使用标准输出比较。距离比较使用欧氏距离的平方;若距离相同,按候选编号升序打破平局。

数据范围与测试提示

  • 正式测试会覆盖样例没有出现的边界输入,请不要依赖样例规模或样例顺序写特判。
  • 输出只包含题目要求的字段,不要输出调试信息;浮点结果按题面要求保留 6 位小数。
  • 坐标链可能只有一级,也可能包含多级旋转和平移,隐藏数据会覆盖连续旋转、负角度和接近 0 的坐标。
  • 每一级变换都要按输入顺序依次作用,不能把平移和旋转交换顺序;最终输出仍按 6 位小数格式化。
  • 候选或阶段编号按输入顺序从 1 开始;多级变换必须严格按输入顺序作用,距离并列取编号小者。

样例

输入样例 1

1 0 0
0 2 0
3 0 0
2
0 0
1 0
4 2

输出样例 1

1 4.000000 2.000000

输入样例 2

0 0 90
1 0 0
0 1 -90
2
1 0
0 1
2 0

输出样例 2

1 1.000000 0.000000