如何实现四元数绕自身轴按指定弧度旋转的函数？

核心原理 #

四元数的旋转不能直接修改单个分量，必须通过四元数乘法组合旋转操作。绕自身局部轴旋转时，需要将当前姿态四元数与局部旋转四元数进行右乘（四元数乘法顺序决定了右乘对应局部空间的旋转叠加），最后归一化避免数值漂移。

完整实现代码 #

import math

class quaternion:
    __slots__ = ("x", "y", "z", "w")

    def __init__(self, x: float, y: float, z: float, w: float):
        self.x = x
        self.y = y
        self.z = z
        self.w = w

    def normalize(self):
        """归一化四元数，避免浮点运算导致的数值漂移"""
        mag = math.sqrt(self.x**2 + self.y**2 + self.z**2 + self.w**2)
        if mag == 0:
            self.x = 0.0
            self.y = 0.0
            self.z = 0.0
            self.w = 1.0
        else:
            inv_mag = 1.0 / mag
            self.x *= inv_mag
            self.y *= inv_mag
            self.z *= inv_mag
            self.w *= inv_mag

    def multiply(self, other: 'quaternion') -> 'quaternion':
        """四元数乘法：self × other"""
        x = self.w * other.x + self.x * other.w + self.y * other.z - self.z * other.y
        y = self.w * other.y - self.x * other.z + self.y * other.w + self.z * other.x
        z = self.w * other.z + self.x * other.y - self.y * other.x + self.z * other.w
        w = self.w * other.w - self.x * other.x - self.y * other.y - self.z * other.z
        return quaternion(x, y, z, w)

    @staticmethod
    def from_axis_angle(x_rad: float, y_rad: float, z_rad: float) -> 'quaternion':
        """从绕X/Y/Z轴的旋转角创建四元数（旋转顺序：X→Y→Z）"""
        half_x = x_rad * 0.5
        sx, cx = math.sin(half_x), math.cos(half_x)

        half_y = y_rad * 0.5
        sy, cy = math.sin(half_y), math.cos(half_y)

        half_z = z_rad * 0.5
        sz, cz = math.sin(half_z), math.cos(half_z)

        # 组合三个轴的旋转四元数，顺序对应欧拉角旋转顺序
        qx = quaternion(sx, 0.0, 0.0, cx)
        qy = quaternion(0.0, sy, 0.0, cy)
        qz = quaternion(0.0, 0.0, sz, cz)

        q_yz = qy.multiply(qx)
        q_final = qz.multiply(q_yz)
        return q_final

    def rotate(self, x_rad: float, y_rad: float, z_rad: float):
        """绕自身局部X/Y/Z轴旋转指定弧度，更新当前四元数"""
        # 创建对应局部旋转的四元数
        rot_quat = quaternion.from_axis_angle(x_rad, y_rad, z_rad)
        # 右乘旋转四元数，叠加局部空间旋转
        new_quat = self.multiply(rot_quat)
        # 归一化后更新自身分量
        new_quat.normalize()
        self.x = new_quat.x
        self.y = new_quat.y
        self.z = new_quat.z
        self.w = new_quat.w

关键细节说明 #

• 轴角转四元数：单个轴旋转的四元数公式为(sin(θ/2), 0, 0, cos(θ/2))（以X轴为例），组合多轴旋转时需严格遵循欧拉角顺序调整乘法顺序。
• 四元数乘法顺序：右乘旋转四元数对应局部轴旋转，左乘则对应世界轴旋转，根据需求切换即可。
• 归一化必要性：浮点运算会导致四元数模长偏离1，必须每次旋转后归一化，否则会出现旋转失真。

使用示例 #

# 初始化无旋转的单位四元数
q = quaternion(0.0, 0.0, 0.0, 1.0)
# 绕自身X轴旋转-0.1弧度（向左旋转）
q.rotate(-0.1, 0.0, 0.0)
# 同时绕自身Y轴转0.2弧度、Z轴转0.05弧度
q.rotate(0.0, 0.2, 0.05)

内容的提问来源于stack exchange，提问作者MirceaKitsune