一、问题
已知原坐标系下的两个点坐标为p1(x1, y1), p2(x2, y2), 经过平移和旋转对应到新坐标系下的点坐标分别为(0, 0), (x, y),计算相应的旋转矩阵R和平移向量T。
二、思路
平移向量一般比较好计算,而旋转矩阵在计算的时候要注意旋转中心点是哪个点,还要注意是平移后旋转还是先旋转后平移。鉴于本问题的特殊性:新坐标系下其中一点的坐标为(0,0),平移向量则可直接相减得出,即T=[0-x1, 0-y1], 然后根据(x2, y2)平移和旋转后对应点为(x, y)可以计算出旋转矩阵。
三、代码
Matrix<double> temp1 = new Matrix<double>(2, 2);
Matrix<double> T = new Matrix<double>(2, 1);
Matrix<double> R = new Matrix<double>(2, 2);
T[0, 0] = 0 - x1;
T[1, 0] = 0 - y1;
temp1[0, 0] = x2 + T[0, 0];
temp1[0, 1] = y2 + T[1, 0];
temp1[1, 0] = y2 + T[1, 0];
temp1[1, 1] = -(x2 + T[0, 0]);
double d = disttance(p1, p2);//计算点p1和p2之间的距离
</double></double><double><double>
Matrix<double> temp2 = new Matrix<double>(2, 1);
temp2[0, 0] = 0;
temp2[1, 0] = d;
Matrix<double> temp3 = inverse(temp1) * (temp2);//此处假设x=0, 则可得y = d, 解此方程得出cos theta, sin theta
R[0, 0] = temp3[0, 0];
R[0, 1] = temp3[1, 0];
R[1, 0] = -temp3[1, 0];
R[1, 1] = temp3[0, 0];</double></double>