四元数和平移

下面是一个使用四元数和平移实现旋转和移动的代码示例：

import numpy as np
import math

class Quaternion:
    def __init__(self, w=1, x=0, y=0, z=0):
        self.w = w
        self.x = x
        self.y = y
        self.z = z

    def normalize(self):
        norm = math.sqrt(self.w**2 + self.x**2 + self.y**2 + self.z**2)
        self.w /= norm
        self.x /= norm
        self.y /= norm
        self.z /= norm

    def conjugate(self):
        return Quaternion(self.w, -self.x, -self.y, -self.z)

    def __mul__(self, q):
        w = self.w * q.w - self.x * q.x - self.y * q.y - self.z * q.z
        x = self.w * q.x + self.x * q.w + self.y * q.z - self.z * q.y
        y = self.w * q.y - self.x * q.z + self.y * q.w + self.z * q.x
        z = self.w * q.z + self.x * q.y - self.y * q.x + self.z * q.w
        return Quaternion(w, x, y, z)

    def rotate(self, v):
        p = Quaternion(0, v[0], v[1], v[2])
        q_conj = self.conjugate()
        q_rotated = self * p * q_conj
        return np.array([q_rotated.x, q_rotated.y, q_rotated.z])
        

def rotate_and_translate(v, rotation, translation):
    q = Quaternion(rotation[0], rotation[1], rotation[2], rotation[3])
    q.normalize()
    rotated_v = q.rotate(v)
    translated_v = rotated_v + translation
    return translated_v

# 示例使用
v = np.array([1, 0, 0])  # 原始向量
rotation = np.array([0.707, 0, 0, 0.707])  # 四元数表示的旋转
translation = np.array([1, 2, 3])  # 平移向量

result = rotate_and_translate(v, rotation, translation)
print(result)

上述代码使用Quaternion类来表示四元数，并实现了四元数的标准运算（乘法、归一化、共轭）和向量的旋转操作。rotate_and_translate函数接受一个向量、一个四元数表示的旋转和一个平移向量，并返回旋转和平移后的向量。示例中，原始向量v进行了绕原点的旋转和平移操作，最终输出旋转和平移后的向量。