Как эффективно найти матрицу вращения из 3-х деформированных точек в Python - PullRequest
Новая
       82

Как эффективно найти матрицу вращения из 3-х деформированных точек в Python

0 голосов
/ 01 февраля 2019

Я пытаюсь найти матрицу вращения между системами координат, используя 3 единичных вектора, которые я определил в обеих системах координат.coordinate rotation visualisation

В настоящее время я использую метод, который я нашел в математике , который определяется следующим образом: 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))

Кто-нибудь знает способ вычисления матрицы вращения с меньшим количеством кода или большей эффективностью?

...