【python】旋转轴角计算旋转矩阵

索引

一、原理
二、源码
三、结果
四、参考

---

一、原理

根据空间内的任意两个坐标点和旋转角度，计算旋转矩阵进行旋转。原理参考博客

绕任意轴旋转的情况比较复杂，主要分为两种情况，一种是平行于坐标轴的，一种是不平行于坐标轴的，对于平行于坐标轴的，我们首先将旋转轴平移至与坐标轴重合，然后进行旋转，最后再平移回去。

将旋转轴平移至与坐标轴重合，对应平移操作
旋转，对应操作
步骤1的逆过程，对应操作

整个过程就是

对于不平行于坐标轴的，可按如下方法处理。（该方法实际上涵盖了上面的情况）

1. 将旋转轴平移至原点
2. 将旋转轴旋转至YOZ平面
3. 将旋转轴旋转至于Z轴重合
4. 绕Z轴旋转θ度
5. 执行步骤3的逆过程
6. 执行步骤2的逆过程
7. 执行步骤1的逆过程

假设用v1(a1, b2, c2)和v2(a2, b2, c2)来表示旋转轴，θ表示旋转角度。为了方便推导，暂时使用右手系并使用列向量，待得出矩阵后转置一下即可，上面步骤对应的流程图如下。

步骤1是一个平移操作，将v1v2平移至原点，对应的矩阵为

步骤2是一个旋转操作，将p(p = v2 -v1)旋转至XOZ平面，步骤3也是一个旋转操作，将p旋转至与Z轴重合，这两个操作对应的图如下。

做点p在平面YOZ上的投影点q。再过q做Z轴垂线，则r是p绕X轴旋转所得，且旋转角度为α，且

，

于是旋转矩阵为

现在将r绕Y轴旋转至与Z轴重合，旋转的角度为-beta（方向为顺时针），且
,

于是得到旋转矩阵为

最后是绕Z轴旋转，对应的矩阵如下

如果旋转轴是过原点的，那么第一步和最后一步的平移操作可以省略，也就是把中间五个矩阵连乘起来，再转置一下，得到下面的绕任意轴旋转的矩阵

即

---

二、源码

"""
    根据旋转轴两个端点、旋转角度，得到旋转矩阵
    v1   旋转轴端点1 (-130.009995,-158.779999,31.349001)
    v2   旋转轴端点2 (-129.216995,-157.925003,31.320999)
    theta 旋转弧度 60度
"""
def rotateMatrix(v1, v2, theta):
    # 向量相减
    p = np.asarray(v2) - np.asarray(v1)
    # 求单位向量
    rand_axis = p / np.linalg.norm(p)
    # 旋转角度
    r = math.radians(theta)
    # 返回旋转矩阵
    rot_matrix = linalg.expm(np.cross(np.eye(3), rand_axis * r))

    return rot_matrix

---

三、结果

---

四、参考

1.绕任意轴旋转