def test_inverse_mult(self): a1 = st.EulerAngle(30, 45, -60).to_rad() q1 = a1.to_quat() a2 = st.EulerAngle(30, 0, 0).to_rad() q2 = a2.to_quat() q3 = q1.quat_mult(q2) q4 = q3.quat_mult(q2.inverse()) a3 = q4.to_euler() assert a3 == a1
def test_mult(self): a1 = st.EulerAngle(30, 45, -60).to_rad() q1 = a1.to_quat() a2 = st.EulerAngle(30, 0, 0).to_rad() q2 = a2.to_quat() q3 = q1.quat_mult(q2) a3 = q3.to_euler() ans = a1 + a2 assert a3 == ans
def test_to_rotmat(self): a1 = st.EulerAngle(30, 45, -60).to_rad() r1 = a1.to_rotmat() ans = st.RotMat() ans.set_rotmat(0.3535534, 0.9267767, -0.1268265, -0.6123725, 0.1268265, -0.7803301, -0.7071068, 0.3535534, 0.6123725) assert r1 == ans
def test_to_rad(self): deg = st.EulerAngle(30, 45, -60) rad = deg.to_rad() ans = st.EulerAngle(0.523599, 0.785398, -1.0472) assert rad == ans
def test_to_axis_angle(self): a1 = st.EulerAngle(30, 45, -60).to_rad() aa1 = a1.to_axis_angle() ans = st.AxisAngle(0.5675524, 0.2904527, -0.7704035, 1.5244035) assert aa1 == ans
def test_to_quat(self): a1 = st.EulerAngle(30, 45, -60).to_rad() q1 = a1.to_quat().round(3) ans = st.Quaternion(0.723, 0.392, 0.201, -0.532) assert q1 == ans
def test_to_deg(self): rad = st.EulerAngle(0.523599, 0.785398, -1.0472) deg = rad.to_deg() ans = st.EulerAngle(30, 45, -60) assert deg == ans
def test_to_euler(self): a1 = st.EulerAngle(30, 45, -60).to_rad() q1 = a1.to_quat() ans = q1.to_euler() assert a1 == ans
from matplotlib import pyplot as plt import numpy as np from mpl_toolkits.mplot3d.axes3d import Axes3D from matplotlib import animation from pathlib import Path import src.spatial_transform as st import src.utils as utils # Setup start_pt = st.Vector3D(0.5, 0.5, 0.5) start_euler = st.EulerAngle(0, 0, 0).to_rad() start_quat = start_euler.to_quat() end_euler = st.EulerAngle(180, 45, -60).to_rad() end_quat = end_euler.to_quat() # Slerp slerp = utils.quat_slerp(start_quat.numpy(), end_quat.numpy(), 0, 1, 0.05) # Calculate points and directions for plotting pos_pts = [start_pt] x_dir_pts = [st.Vector3D(1, 0, 0)] y_dir_pts = [st.Vector3D(0, 1, 0)] z_dir_pts = [st.Vector3D(0, 0, 1)] euler_angles = [start_euler.to_deg()] new_quat = st.Quaternion() new_quat.from_numpy(slerp[-1]) new_y_dir = y_dir_pts[-1].rotate_by_quat(new_quat)
def test_to_euler(self): a1 = st.EulerAngle(30, 45, -60).to_rad() r1 = a1.to_rotmat() ans = r1.to_euler() assert a1 == ans
def test_to_euler(self): aa1 = st.AxisAngle(0.5675524, 0.2904527, -0.7704035, 1.5244035) a1 = aa1.to_euler() ans = st.EulerAngle(0.5235988, 0.7853981, -1.0471976) assert a1 == ans