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