用四元数旋转三维空间中的点

写了两个代码，第一个是完整的实现使用四元数旋转空间点的算法。（将（1，0，0）绕Y轴旋转90度）

第二个代码是使用scipy.spatial.transform.Rotation库旋转空间点。这里调用的库也可以选择 numpy-quaternion。

工程上更推荐第二种，第一种仅用来学习。

自定义使用四元数旋转空间点

import numpy as np

def quaternion_rotation(point, axis, angle):
    """
    使用四元数旋转空间点
    :param point: 待旋转的点 (x, y, z)
    :param axis: 旋转轴 (x, y, z)，需要归一化
    :param angle: 旋转角度（单位：弧度）
    :return: 旋转后的点 (x', y', z')
    """
    # 确保旋转轴是单位向量
    axis = axis / np.linalg.norm(axis)
    
    # 四元数构造：q = [w, x, y, z]
    w = np.cos(angle / 2)
    x, y, z = axis * np.sin(angle / 2)
    quaternion = np.array([w, x, y, z])
    
    # 待旋转点的齐次表示，添加一个 w=0 的分量
    p = np.array([0] + list(point))
    
    # 四元数乘法规则：q * p * q_conj
    def quaternion_multiply(q1, q2):
        w1, x1, y1, z1 = q1
        w2, x2, y2, z2 = q2
        w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
        x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
        y = w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2
        z = w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2
        return np.array([w, x, y, z])
    
    # 四元数的共轭
    q_conj = np.array([quaternion[0], -quaternion[1], -quaternion[2], -quaternion[3]])
    
    # 计算 q * p * q_conj
    p_rotated = quaternion_multiply(quaternion_multiply(quaternion, p), q_conj)
    
    # 返回旋转后的点（去掉 w 分量）
    return p_rotated[1:]

# 测试
point = np.array([1, 0, 0])  # 待旋转点
axis = np.array([0, 1, 0])   # 绕 Y 轴旋转
angle = np.pi / 2            # 旋转 90 度

rotated_point = quaternion_rotation(point, axis, angle)
print("自定义四元数旋转结果：", np.round(rotated_point, decimals=6))
# print("自定义四元数旋转结果：", rotated_point)

使用scipy.spatial.transform.Rotation库旋转空间点

import numpy as np  # 添加这一行以导入 numpy
from scipy.spatial.transform import Rotation as R

# 测试
point = [1, 0, 0]  # 待旋转点
axis = [0, 1, 0]   # 绕 Y 轴旋转
angle = np.pi / 2  # 旋转 90 度

# 创建旋转对象
rotation = R.from_rotvec(angle * np.array(axis))

# 旋转点
rotated_point = rotation.apply(point)
print("自定义四元数旋转结果：", np.round(rotated_point, decimals=6))