def test_finte():
q1 = Quat(10000000000, 210000000000, 310000000000, -2147483647)
q2 = Quat(np.nan, 210000000000, 310000000000, -2147483647)
q3 = Quat(np.inf, 210000000000, 310000000000, -2147483647)
q4 = Quat(0.0, 210000000000, np.NINF, -2147483647)
v1 = Vec3(10000000000, 210000000000, 310000000000)
v2 = Vec3(np.nan, 210000000000, 310000000000)
v3 = Vec3(np.inf, 210000000000, 310000000000)
v4 = Vec3(0.0, 210000000000, np.NINF)
t1 = Transform(q1, v1)
assert t1.is_finite()
assert not t1.is_nan()
t2 = Transform(q1, v2)
assert not t2.is_finite()
assert t2.is_nan()
t3 = Transform(q3, v1)
assert not t3.is_finite()
assert t3.is_nan()
t4 = Transform(q4, v4)
assert not t4.is_finite()
assert t4.is_nan()
def test_rotate_vector():
v = Vec3(1, 1, 0)
q1 = Quat.from_rotation_on_axis(0, np.pi / 3)
q2 = Quat.from_rotation_on_axis(1, np.pi / 3)
q3 = Quat.from_rotation_on_axis(2, np.pi / 3)
t1 = Transform(q1, Vec3.zero())
t2 = Transform(q2, Vec3.zero())
t3 = Transform(q3, Vec3.zero())
# forward rotation
v1 = t1.rotate(v)
v2 = t2.rotate(v)
v3 = t3.rotate(v)
theta1 = (60) * np.pi / 180.0
v1_true = Vec3(1, math.cos(theta1), math.sin(theta1))
v2_true = Vec3(math.cos(theta1), 1, -math.sin(theta1))
theta2 = (45 + 60) * np.pi / 180.0
v3_true = math.sqrt(2) * Vec3(math.cos(theta2), math.sin(theta2), 0)
assert v1 == v1_true
assert v2 == v2_true
assert v3 == v3_true
# inverse rotate
v1_rev = t1.inverse_rotate(v1_true)
v2_rev = t2.inverse_rotate(v2_true)
v3_rev = t3.inverse_rotate(v3_true)
assert v == v1_rev
assert v == v2_rev
assert v == v3_rev
def test_dot():
q1 = Quat(0, 1, 2, 3)
q2 = Quat(3, 4, 5, 6)
res = q1.dot(q2)
assert res == 32
def test_rotate_vec():
v = Vec3(1, 1, 0)
q1 = Quat.from_rotation_on_axis(0, np.pi / 3)
q2 = Quat.from_rotation_on_axis(1, np.pi / 3)
q3 = Quat.from_rotation_on_axis(2, np.pi / 3)
# forward rotation
v1 = q1.rotate(v)
v2 = q2.rotate(v)
v3 = q3.rotate(v)
theta1 = (60) * np.pi / 180.0
v1_true = Vec3(1, math.cos(theta1), math.sin(theta1))
v2_true = Vec3(math.cos(theta1), 1, -math.sin(theta1))
theta2 = (45 + 60) * np.pi / 180.0
v3_true = math.sqrt(2) * Vec3(math.cos(theta2), math.sin(theta2), 0)
assert v1 == v1_true
assert v2 == v2_true
assert v3 == v3_true
# inverse rotate
v1_rev = q1.inverse_rotate(v1_true)
v2_rev = q2.inverse_rotate(v2_true)
v3_rev = q3.inverse_rotate(v3_true)
assert v == v1_rev
assert v == v2_rev
assert v == v3_rev
def test_normalize():
q1 = Quat(0.4619398, 0.1913417, 0.4619398, 0.7325378) # 45 around X then Y then Z
t1 = Transform(q1, Vec3.zero())
assert t1.is_normalized()
q2 = Quat(3, 0, 4, 7)
t2 = Transform(q2, Vec3.zero())
assert not t2.is_normalized()
t2.normalize()
assert t2.is_normalized()
def test_distance():
q1 = Quat(1.0, 2.0, 3.0, 4)
q2 = Quat(3, 0, 4, 5)
assert q1.distance(q2) == math.sqrt(10)
assert q1.distance_squared(q2) == 10
assert q2.distance(q1) == math.sqrt(10)
assert q2.distance_squared(q1) == 10
assert q2.distance(q2) == 0
def test_div():
q1 = Quat(1, 2, 3, -5)
q2 = q1 / -4
assert isinstance(q2, Quat)
q_true = Quat(-0.25, -0.5, -0.75, 1.25)
assert q2 == q_true
q1 /= -4
assert q1 == q_true
def test_normalize():
q1 = Quat(0.4619398, 0.1913417, 0.4619398,
0.7325378) # 45 around X then Y then Z
assert q1.is_normalized()
q2 = Quat(3, 0, 4, 7)
assert not q2.is_normalized()
q2.normalize()
assert q2.is_normalized()
def rotateFromQuat(self, q):
"""Function rotateFromQuat from the Vec3 class rotates the current vector by the rotation
represented by the Quat q. This is also the adjoint map for S^3
Example:
>>> v = Vec3(1.,1.,1.)
>>> q = Quat.(0.707,0.,0.,0.707)
>>> v.rotateFromQuat(q)
>>> print(v)
[1.,-1,1.]
"""
from quat import Quat
self.put(range(3),(Quat.product(q,Quat.product(Quat(numpy.hstack((self, [0.]))), q.getInverse()))).getIm())
def test_add():
q1 = Quat(1, 1, 1, 1)
q2 = Quat(2, 2, 2, 2)
q3 = q1 + q2
assert isinstance(q3, Quat)
q_true = Quat(3, 3, 3, 3)
assert q3 == q_true
q1 += q2
assert q1 == q_true
def test_length():
q1 = Quat(1.0, 2.0, 3.0, 4)
q2 = Quat(3, 0, 4, 5)
assert q1.length() == math.sqrt(30)
assert q1.length_squared() == 30
assert q2.length() == math.sqrt(50)
assert q2.length_squared() == 50
def test_mul():
q1 = Quat(1, 2, 3, -5)
q2 = q1 * -4
q3 = -4 * q1
assert isinstance(q2, Quat)
assert isinstance(q3, Quat)
assert q2 == q3
q_true = Quat(-4, -8, -12, 20)
assert q2 == q_true
q1 *= -4
assert q1 == q_true
def test_sub():
q1 = Quat(1, 1, 1, 1)
q2 = Quat(4, 4, 4, 4)
q3 = q1 - q2
assert isinstance(q3, Quat)
q_true = Quat(-3, -3, -3, -3)
assert q3 == q_true
q1 -= q2
assert q1 == q_true
def test_from_numpy():
m = np.array([
[-0.1390883, 0.9896649, 0.0348995],
[0.8625845, 0.1037672, 0.4951569],
[0.4864179, 0.0989743, -0.8681024],
])
q = np.array([1, 2, 3, 4])
qm_true = Quat(0.6374259, 0.7264568, 0.204462, -0.1553838)
qq_true = Quat(1, 2, 3, 4)
qm = Quat.from_numpy(m)
qq = Quat.from_numpy(q)
assert qm == qm_true
assert qq == qq_true
def test_slerp():
q1 = Quat.from_rotation_on_axis(2, -np.pi / 4)
q2 = Quat.from_rotation_on_axis(2, np.pi / 4)
q3 = q1.slerp(q2, 0.5)
q4 = q2.slerp(q1, 0.5)
q_true = Quat.identity()
assert q3 == q_true
assert q4 == q_true
q5 = q1.slerp(q2, 0.9)
q_true = Quat.from_rotation_on_axis(2, (45 - 9) * np.pi / 180)
assert q5 == q_true
def test_init_and_eq_and_ne():
t1 = Transform()
assert t1.rotation == Quat.identity()
assert t1.translation == Vec3.zero()
rot = Quat.from_rotation_on_axis(2, np.pi)
trans = Vec3(1, 2, 3)
t2 = Transform(rot, trans)
assert t2.rotation == rot
assert t2.translation == trans
t3 = Transform.identity()
assert t1 == t3
assert t1 != t2
assert t3 != t2
def getMatrix(self):
"""Returns the convertion of the quaternion into rotation matrix form.
"""
q = Quat(self)
# Repetitive calculations
q44 = q[3]**2
q12 = q[0] * q[1]
q13 = q[0] * q[2]
q14 = q[0] * q[3]
q23 = q[1] * q[2]
q24 = q[1] * q[3]
q34 = q[2] * q[3]
matrix = numpy.empty((3, 3))
# The diagonal
matrix[0, 0] = 2.0 * (q[0]**2 + q44) - 1.0
matrix[1, 1] = 2.0 * (q[1]**2 + q44) - 1.0
matrix[2, 2] = 2.0 * (q[2]**2 + q44) - 1.0
# Off-diagonal
matrix[0, 1] = 2.0 * (q12 - q34)
matrix[0, 2] = 2.0 * (q13 + q24)
matrix[1, 2] = 2.0 * (q23 - q14)
matrix[1, 0] = 2.0 * (q12 + q34)
matrix[2, 0] = 2.0 * (q13 - q24)
matrix[2, 1] = 2.0 * (q23 + q14)
return matrix
def test_numpy_interop():
rot = Quat.from_rotation_on_axis(2, np.pi / 2)
trans = Vec3(1, 1, 1)
# fmt: off
np_T = np.array([
[0, -1, 0, 1],
[1, 0, 0, 1],
[0, 0, 1, 1],
[0, 0, 0, 1],
])
# fmt: on
T_a_b = Transform(rot, trans)
T_a_b_from_mat4 = Transform.from_mat4_numpy(np_T)
T_a_b_from_quatvec3 = Transform.from_quat_vec3_numpy(
np.concatenate((rot.to_xyzw_numpy(), trans.to_numpy()))
)
assert T_a_b == T_a_b_from_mat4
assert T_a_b == T_a_b_from_quatvec3
assert T_a_b_from_quatvec3 == T_a_b_from_mat4
np_mat4_T = T_a_b.to_mat4_numpy()
np_quatvec3_T = T_a_b.to_quat_vec3_numpy()
assert np.allclose(
np_quatvec3_T, np.concatenate((rot.to_xyzw_numpy(), trans.to_numpy()))
)
assert np.allclose(np_mat4_T, np_T)
def test_translate_vector():
v = Vec3(1, 1, 0)
t = Transform(Quat.identity(), Vec3(4, 5, 6))
res = t.translate(v)
assert res == Vec3(5, 6, 6)
def ori(self):
"""Gets orientation property of this transform.
Returns:
float: Orientation property of this transform.
"""
return Quat(self._rtval.ori)
def getConjugate(self):
"""Returns the conjugate of the quaternion.
Example:
>>> q = Quat(0.707,0.,0.,0.707)
>>> q.getConjugate()
[-0.707,0.,0.,0.707]
"""
return Quat(-self.take(0), -self.take(1), -self.take(2), self.take(3))
def createFromVectors(v1, v2):
""" Function createFromVectors expects two 3d vectors. Quat has the Sofa format i.e (x,y,z,w).
Examples:
>>> q = Quat.createFromVectors([1,0,0],[0,1,0])
>>> print(q)
[0.,0.,0.707,0.707]
"""
from quat import Quat
from vec3 import Vec3
from math import sqrt
q = Quat()
v = Vec3.cross(v1, v2)
q[0:3] = v
q[3] = sqrt((Vec3(v1).getNorm()**2) *
(Vec3(v2).getNorm()**2)) + Vec3.dot(v1, v2)
q.normalize()
return q
def test_xyzw_numpy():
a = np.array([1, 2, 3, 4])
q1 = Quat.from_xyzw_numpy(a)
b = q1.to_xyzw_numpy()
assert a[0] == q1.x
assert a[1] == q1.y
assert a[2] == q1.z
assert a[3] == q1.w
assert np.allclose(a, b)
def test_angle():
q1 = Quat(0.7071068, 0, 0, 0.7071068) # 90 degrees around x axis
q2 = Quat(0, 0.3826834, 0, 0.9238795) # 45 around y axis
assert abs(q1.angle() - np.pi / 2) < MATH_EPS
assert abs(q2.angle() == np.pi / 4) < MATH_EPS
res = q1.angle(q2)
assert abs(res - 1.7177715322714415) < MATH_EPS
def createFromAxisAngle(axis, angle):
""" Function createQuatFromAxis from quat expects two arguments. Quat has the Sofa format i.e (x,y,z,w).
Examples:
>>> q = Quat.createQuatFromAxis([1.,0.,0.],pi/2.)
>>> print(q)
[0.707,0.,0.,0.707]
Note that the angle should be in radian.
"""
from quat import Quat
q = Quat()
q[0] = axis[0] * math.sin(angle / 2.)
q[1] = axis[1] * math.sin(angle / 2.)
q[2] = axis[2] * math.sin(angle / 2.)
q[3] = math.cos(angle / 2.)
q.normalize()
return q
def test_rotateFromQuat(self):
from quat import Quat
from math import pi
v = Vec3(1., 1., 1.)
q = Quat.createFromAxisAngle(Vec3([1., 0., 0.]), pi / 2.)
v.rotateFromQuat(q)
self.assertEqual(v[0], 1.)
self.assertEqual(math.floor(v[1]), -1.)
self.assertEqual(v[2], 1.)
def test_mul():
T_a_b = Transform(Quat.from_rotation_on_axis(2, np.pi / 2), Vec3(1, 1, 1))
T_b_c = Transform(Quat.from_rotation_on_axis(0, np.pi / 2), Vec3(-1, -2, -3))
c_p = Vec3(2, 3, 4)
T_a_c = T_a_b * T_b_c
assert isinstance(T_a_c, Transform)
a_p = T_a_c * c_p
assert isinstance(a_p, Vec3)
# fmt: off
np_T_a_b = np.array([
[0, -1, 0, 1],
[1, 0, 0, 1],
[0, 0, 1, 1],
[0, 0, 0, 1],
])
np_T_b_c = np.array([
[1, 0, 0, -1],
[0, 0, -1, -2],
[0, 1, 0, -3],
[0, 0, 0, 1],
])
np_c_p = np.array([
[2],
[3],
[4],
[1],
])
# fmt: on
npgt_T_a_c = np_T_a_b @ np_T_b_c
npgt_a_p = np.squeeze(npgt_T_a_c @ np_c_p)[0:3]
np_T_a_c = T_a_c.to_mat4_numpy()
np_a_p = a_p.to_numpy()
assert np.allclose(np_T_a_c, npgt_T_a_c)
assert np.allclose(npgt_a_p, np_a_p)
def test_rotation_vector():
# Generated from - https://www.andre-gaschler.com/rotationconverter/
# 36 degrees around axis (1, 2, 3)
q1 = Quat(0.0825883, 0.1651765, 0.2477648, 0.9510565)
v = q1.to_rotation_vector()
theta = v.length()
axis = v.normalized()
theta_true = 36 * np.pi / 180
axis_true = Vec3(1 / math.sqrt(14), 2 / math.sqrt(14), 3 / math.sqrt(14))
assert abs(theta - theta_true) < MATH_EPS
assert axis == axis_true
rotation_vec = theta_true * axis_true
q2 = Quat.from_rotation_vector(rotation_vec)
assert q1 == q2
q3 = Quat.from_axis_angle(axis_true, theta_true)
assert q1 == q3
def rotateFromEuler(self, v, axis="sxyz"):
"""Function rotateFromEuler from the Vec3 class rotates the current vector from Euler angles [x,y,z].
Example:
>>> v = Vec3(1.,1.,1.)
>>> v.rotateFromEuler([pi/2.,0.,0.])
>>> print(v)
[1.,-1,1.]
"""
from quat import Quat
q = Quat.createFromEuler(v, axis)
self.rotateFromQuat(q)
def test_euler_angles():
# Generated from - https://www.andre-gaschler.com/rotationconverter/
# 30 degrees around X, 9 around Y, 153 around Z
q1 = Quat(0.1339256, -0.2332002, 0.9410824, 0.2050502)
x, y, z = q1.get_euler_angles(angle_order=[0, 1, 2])
assert abs(x - np.pi / 6) < MATH_EPS
assert abs(y - np.pi / 20) < MATH_EPS
assert abs(z - 153 * np.pi / 180) < MATH_EPS
x, z, y = q1.get_euler_angles(angle_order=[0, 2, 1])
x_true, y_true, z_true = -2.6975331, 2.9656712, 0.4649757
assert abs(x - x_true) < MATH_EPS
assert abs(y - y_true) < MATH_EPS
assert abs(z - z_true) < MATH_EPS
y, x, z = q1.get_euler_angles(angle_order=[1, 0, 2])
x_true, y_true, z_true = 0.5165051, 0.1808875, 2.7604268
assert abs(x - x_true) < MATH_EPS
assert abs(y - y_true) < MATH_EPS
assert abs(z - z_true) < MATH_EPS
yaw, pitch, roll = q1.get_yaw_pitch_roll()
assert abs(pitch - x_true) < MATH_EPS
assert abs(yaw - y_true) < MATH_EPS
assert abs(roll - z_true) < MATH_EPS
y, z, x = q1.get_euler_angles(angle_order=[1, 2, 0])
x_true, y_true, z_true = 2.5925117, -2.7653147, 0.3293999
assert abs(x - x_true) < MATH_EPS
assert abs(y - y_true) < MATH_EPS
assert abs(z - z_true) < MATH_EPS
z, x, y = q1.get_euler_angles(angle_order=[2, 0, 1])
x_true, y_true, z_true = -0.3941227, -0.3860976, 2.6345058
assert abs(x - x_true) < MATH_EPS
assert abs(y - y_true) < MATH_EPS
assert abs(z - z_true) < MATH_EPS
z, y, x = q1.get_euler_angles(angle_order=[2, 1, 0])
x_true, y_true, z_true = -0.4219639, -0.3551227, 2.7893517
assert abs(x - x_true) < MATH_EPS
assert abs(y - y_true) < MATH_EPS
assert abs(z - z_true) < MATH_EPS
