import numpy as np
A0 = np.array([0.4874625, 0.8284295, 0.135549])
B0 = np.array([0.488733, 0.872464, 0.150309])
C0 = np.array([0.6290205, 0.817183, 0.126746])
A1 = np.array([0.6979735, 0.823703, 0.222408])
B1 = np.array([0.706303, 0.86421, 0.222813])
C1 = np.array([0.7162125, 0.8322515, 0.0745535])
P = np.stack([B0 - A0, C0 - A0], axis=1)
Q = np.stack([B1 - A1, C1 - A1], axis=1)
H = P @ Q.T
U, S, Vt = np.linalg.svd(H)
V = Vt.T
R = V @ U.T
if np.linalg.det(R) < 0:
V[:, -1] *= -1
R = V @ U.T
sy = np.sqrt(R[0,0]**2 + R[1,0]**2)
singular = sy < 1e-6
if not singular:
yaw = np.arctan2(R[1,0], R[0,0])
pitch = np.arctan2(-R[2,0], sy)
roll = np.arctan2(R[2,1], R[2,2])
else:
yaw = np.arctan2(-R[0,1], R[0,2])
pitch = np.arctan2(-R[2,0], sy)
roll = 0
yaw_deg = np.degrees(yaw)
pitch_deg = np.degrees(pitch)
roll_deg = np.degrees(roll)
print(f"Yaw: {yaw_deg:.2f}°, Pitch: {pitch_deg:.2f}°, Roll: {roll_deg:.2f}°")