3D空间中正方形基于角点位移的各轴旋转计算方法问询

因为正方形未发生形变，所以顶点的位移属于刚体变换（仅包含平移和旋转），我们可以先消除平移的影响，再通过正交基的变化推导旋转矩阵，最后从旋转矩阵中提取各坐标轴的旋转角度（欧拉角）。具体步骤如下：

步骤1：消除平移影响 #

平移不会改变旋转角度，所以先将初始和变换后的点集转换到同一局部坐标系：

• 选取初始正方形的任意一个顶点作为基准点，比如 ( P_0 = (x_0, y_0, z_0) )，将所有初始顶点减去 ( P_0 )，得到初始局部点集：
  ( P_{1,\text{local}} = P_1 - P_0 )，( P_{2,\text{local}} = P_2 - P_0 )，( P_{3,\text{local}} = P_3 - P_0 )
• 对应变换后的基准点 ( P_0' = (x_0', y_0', z_0') )，将所有变换后的顶点减去 ( P_0' )，得到变换后局部点集：
  ( P_{1,\text{local}}' = P_1' - P_0' )，( P_{2,\text{local}}' = P_2' - P_0' )，( P_{3,\text{local}}' = P_3' - P_0' )

步骤2：构建初始局部正交基 #

从初始正方形的邻边提取正交向量，构建局部坐标系：

• 取两条邻边的向量：( \vec{v_1} = P_{1,\text{local}} - P_{0,\text{local}} )（( P_0-P_1 ) 为正方形的一条边），( \vec{v_2} = P_{3,\text{local}} - P_{0,\text{local}} )（( P_0-P_3 ) 为相邻边）
• 验证：因为是正方形，( \vec{v_1} \cdot \vec{v_2} = 0 )（正交）且 ( |\vec{v_1}| = |\vec{v_2}| )（边长相等）
• 单位化向量：( \vec{u_1} = \vec{v_1} / |\vec{v_1}| )，( \vec{u_2} = \vec{v_2} / |\vec{v_2}| )
• 计算第三个正交向量（右手坐标系）：( \vec{u_3} = \vec{u_1} \times \vec{u_2} )（叉乘）
• 初始基矩阵 ( R_0 )：以 ( \vec{u_1}, \vec{u_2}, \vec{u_3} ) 为列向量，即 ( R_0 = \begin{bmatrix} \vec{u_1} & \vec{u_2} & \vec{u_3} \end{bmatrix} )

步骤3：构建变换后局部正交基 #

用同样的方法处理变换后的点集：

• 对应邻边向量：( \vec{v_1}' = P_{1,\text{local}}' - P_{0,\text{local}}' )，( \vec{v_2}' = P_{3,\text{local}}' - P_{0,\text{local}}' )
• 单位化：( \vec{u_1}' = \vec{v_1}' / |\vec{v_1}'| )，( \vec{u_2}' = \vec{v_2}' / |\vec{v_2}'| )
• 计算第三个正交向量：( \vec{u_3}' = \vec{u_1}' \times \vec{u_2}' )
• 变换后基矩阵 ( R_1 )：( R_1 = \begin{bmatrix} \vec{u_1}' & \vec{u_2}' & \vec{u_3}' \end{bmatrix} )

步骤4：计算旋转矩阵 #

初始基到变换后基的旋转矩阵 ( R ) 满足：( R_1 = R \times R_0 )。由于 ( R_0 ) 是正交矩阵（列向量正交单位化），其逆矩阵等于转置矩阵，因此：

R = R_1 × R_0^T

步骤5：从旋转矩阵提取欧拉角 #

欧拉角的提取依赖于旋转顺序（常用的有Z-Y-X、X-Y-Z等），以下以**Z-Y-X顺序（偏航ψ-俯仰θ-滚转φ）**为例：

• 俯仰角 ( \theta = \arcsin(-R[2][0]) )
• 滚转角 ( \phi = \arctan2(R[2][1], R[2][2]) )
• 偏航角 ( \psi = \arctan2(R[1][0], R[0][0]) )

注意事项 #

• 必须保证顶点的对应关系正确（初始的 ( P_i ) 对应变换后的 ( P_i' )），否则结果完全错误
• 变换前后的边长需一致，可作为验证步骤（计算 ( |\vec{v_1}| ) 和 ( |\vec{v_1}'| ) 是否相等）
• 浮点数计算存在精度误差，判断正交时允许微小偏差（比如点积绝对值小于 ( 1e-6 ) 即认为正交）

内容的提问来源于stack exchange，提问作者ejc