Finding the rotation matrix between two vectors in Python

早过忘川 提交于 2021-01-29 07:09:30

问题


I am trying to write a code that gives me the rotation matrix between two vectors. I have tried the code given as an answer to this question. I first make it calculate the rotation matrix and then test it to see if it gives the correct answer. However, it doesn't seem to be giving the correct answer. Could anyone help with why I can't get the correct answer with this?

The Code is below:

import numpy as np 

def rotation_matrix_from_vectors(vec1, vec2):

    a, b = (vec1 / np.linalg.norm(vec1)).reshape(3), (vec2 / np.linalg.norm(vec2)).reshape(3)
    v = np.cross(a, b)
    c = np.dot(a, b)
    s = np.linalg.norm(v)
    kmat = np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])
    rotation_matrix = np.eye(3) + kmat + kmat.dot(kmat) * ((1 - c) / (s ** 2))
    return rotation_matrix

#from here it is the code I have written to test this function
vector1 = array([0,1,0])
vector2 = array([-1,0,1])

rot = rotation_matrix_from_vectors(vector1,vector2)
rotated = np.dot(rot,vector1)

print(rotated)

This prints vector [-0.707, 0, 0.707] instead of [-1, 0, 1].

来源:https://stackoverflow.com/questions/63525482/finding-the-rotation-matrix-between-two-vectors-in-python

易学教程内所有资源均来自网络或用户发布的内容,如有违反法律规定的内容欢迎反馈
该文章没有解决你所遇到的问题?点击提问,说说你的问题,让更多的人一起探讨吧!