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