如何从python中的3个变形点中找到旋转矩阵

时间:2019-02-01 03:45:45

标签: python numpy math linear

我试图通过使用在两个坐标系中定义的3个单位矢量找到坐标系之间的旋转矩阵。 coordinate rotation visualisation

我当前正在使用method I found on Mathmatics,其定义如下: rotation matrix equation

在python中实现此功能的最小工作示例如下:

p1 = [math.sqrt(2)/2,math.sqrt(2)/2,0]
p2 = [-math.sqrt(2)/2,math.sqrt(2)/2,0]
p3 = [0,0,1]
TT = np.vstack([p1, p2, p3]).T
TM = np.eye(3)
for i in range (0,2):
    for j in range(0,2):
        TM[i][j] = math.cos(angle_between_vectors(np.eye(3)[:,j],TT[i,:]))


def vector_norm(data, axis=None, out=None):
    data = np.array(data, dtype=np.float64, copy=True)
    if out is None:
        if data.ndim == 1:
            return math.sqrt(np.dot(data, data))
        data *= data
        out = np.atleast_1d(np.sum(data, axis=axis))
        np.sqrt(out, out)
        return out
    else:
        data *= data
        np.sum(data, axis=axis, out=out)
        np.sqrt(out, out)

def angle_between_vectors(v0, v1, directed=True, axis=0):
    v0 = np.array(v0, dtype=np.float64, copy=False)
    v1 = np.array(v1, dtype=np.float64, copy=False)
    dot = np.sum(v0 * v1, axis=axis)
    dot /= np.linalg.norm(v0, axis=axis) * vector_norm(v1, axis=axis)
    dot = np.clip(dot, -1.0, 1.0)
    return np.arccos(dot if directed else np.fabs(dot)) 

有人知道更少的代码或更高的效率来计算旋转矩阵的方法吗?

0 个答案:

没有答案