坐标旋转与平移

一、问题

已知原坐标系下的两个点坐标为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>