import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from scipy.spatial.transform import Rotation as R

# 定义你的坐标系（原点 + 欧拉角，单位为角度）
frames = [
    {"origin": [0, 0, 0], "euler": [0, 0, 0]},
    {"origin": [2, 2, 2], "euler": [0, 150, 90]},
    {"origin": [3, 3, 3], "euler": [-180, -30, 0]},
]

def draw_frame(ax, origin, rot_matrix, length=0.5):
    origin = np.array(origin)
    x_axis = origin + rot_matrix @ np.array([length, 0, 0])
    y_axis = origin + rot_matrix @ np.array([0, length, 0])
    z_axis = origin + rot_matrix @ np.array([0, 0, length])
    ax.quiver(*origin, *(x_axis - origin), color='r')
    ax.quiver(*origin, *(y_axis - origin), color='g')
    ax.quiver(*origin, *(z_axis - origin), color='b')

fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.set_box_aspect([1,1,1])

for frame in frames:
    origin = frame["origin"]
    euler_deg = frame["euler"]
    rotation = R.from_euler('xyz', euler_deg, degrees=True)
    rot_matrix = rotation.as_matrix()
    draw_frame(ax, origin, rot_matrix)

ax.set_xlim(-1, 3)
ax.set_ylim(-1, 3)
ax.set_zlim(-1, 3)
ax.set_xlabel("X")
ax.set_ylabel("Y")
ax.set_zlabel("Z")
plt.title("Multiple 3D Coordinate Frames (Euler Angles)")
plt.show()