围绕模型自己的轴旋转模型

import numpy as np

# 定义一个四元数
def quaternion_from_axis_angle(axis, angle):
    axis = axis / np.linalg.norm(axis)
    half_angle = angle / 2.0
    q = np.array([np.cos(half_angle), axis[0] * np.sin(half_angle), axis[1] * np.sin(half_angle), axis[2] * np.sin(half_angle)])
    return q

# 定义一个点
point = np.array([1, 0, 0])

# 定义旋转轴和角度
axis = np.array([0, 1, 0])  # 绕Y轴旋转
angle = np.pi / 2  # 旋转90度

# 计算四元数
q = quaternion_from_axis_angle(axis, angle)

# 定义四元数乘法
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 + y1 * w2 + z1 * x2 - x1 * z2
    z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2
    return np.array([w, x, y, z])

# 定义点转换为四元数
def point_to_quaternion(p):
    return np.array([0, p[0], p[1], p[2]])

# 定义四元数转换为点
def quaternion_to_point(q):
    return q[1:]

# 旋转点
q_inv = np.array([q[0], -q[1], -q[2], -q[3]])  # 四元数的逆
p_quat = point_to_quaternion(point)
rotated_p_quat = quaternion_multiply(quaternion_multiply(q, p_quat), q_inv)
rotated_point = quaternion_to_point(rotated_p_quat)

print("旋转后的点:", rotated_point)