已知向量旋转到新向量 / 向量绕轴旋转--四元数使用的python实现

参考链接：

python_四元数/旋转矩阵/欧拉角之间的相互变换(简单易用版)_惊鸿一博-CSDN博客

旋转变换（三）四元数_Frank的专栏-CSDN博客_旋转四元数

scipy.spatial.transform.Rotation — SciPy v1.7.1 Manual

https://krasjet.github.io/quaternion/quaternion.pdf

目标：

已知一个向量 fromVector，要旋转成为一个新向量 toVector。求旋转过程中的四元数。

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import cv2
import numpy as np
import math
import random
from scipy.spatial.transform import Rotation as R

model = 'zyx'
class QuaternionHandler:
    def __init__(self):
        pass
    def main(self):
        pass

    # 已知 起始向量 最终向量，获取旋转的四元数
    # 要注意的要点：
    # 1. 算中心轴的方向：toVector 叉乘 fromVector
    # 2. 绕轴转的方向，以逆时针为正方向
    # 3. 四元数的组成 [Ux*sin(theta/2), Uy*sin(theta/2), Uz*sin(theta/2), cos(theta/2)]
    # 4. [Ux, Uy, Uz]必须是单位向量
    def getQuaternion(self, fromVector, toVector):
        fromVector = np.array(fromVector)
        fromVector_e = fromVector / np.linalg.norm(fromVector)

        toVector = np.array(toVector)
        toVector_e = toVector / np.linalg.norm(toVector)

        cross = np.cross(fromVector_e, toVector_e)

        cross_e = cross / np.linalg.norm(cross)

        dot = np.dot(fromVector_e, toVector_e)

        angle = math.acos(dot)


        if angle == 0 or angle == math.pi:
            print("两个向量处于一条直线")
            return False
        else:
            return [cross_e[0]*math.sin(angle/2), cross_e[1]*math.sin(angle/2), cross_e[2]*math.sin(angle/2), math.cos(angle/2)]

    # 已知四元数，求欧拉角
    def getEuler(self, quaternion):
        rotationClass = self.getRotationClass(quaternion)
        # euler = rotationClass.as_euler(model, degrees=False)
        euler = rotationClass.as_euler(model, degrees=True)
        return euler

    # 已知四元数，求旋转矩阵
    def getRotationClass(self, quaternion):
        rotation = R.from_quat(quaternion)
        return rotation

    def getRotation(self, quaternion):
        rotationClass = self.getRotationClass(quaternion)
        return rotationClass.as_matrix()

if __name__ == '__main__':
    q = QuaternionHandler()
    fromV = np.array([1,0,0])
    toV = np.array([1,0,1])

    print('toV')
    print(np.linalg.norm(toV))
    Rq1 = q.getQuaternion(fromV, toV)
    print(Rq1)
    print(np.linalg.norm(np.array(Rq1)))



    # Rq1 = [0.71934025092983234, -1.876085535681999e-06, -3.274841213980097e-08, -0.69465790385533299]
    rC = q.getRotationClass(Rq1)
    r = q.getRotation(Rq1)
    print(r)
    print('test')
    print(np.matmul(r, fromV.T))

    print(rC.apply(fromV))
    print('apply(fromV)')


    e = q.getEuler(Rq1)
    print(e)

如代码，使用

scipy.spatial.transform。其中的注意事项，已经写在代码中。

要注意的要点：
1. 算中心轴的方向：toVector 叉乘 fromVector
2. 绕轴转的方向，以逆时针为正方向
3. 四元数的组成 [Ux*sin(theta/2), Uy*sin(theta/2), Uz*sin(theta/2), cos(theta/2)]
4. [Ux, Uy, Uz]必须是单位向量，从而四元数的模也是1

2022-11-10 修改：

1.之前叉乘搞反了，这次修改好了。感谢回复中大家的指正。

2.四元数的旋转是逆时针为正方向。

3. Quaternions - Visualisation 这个网站可以直观的介绍四元数的效果。