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_dot():
q1 = Quat(0, 1, 2, 3)
q2 = Quat(3, 4, 5, 6)
res = q1.dot(q2)
assert res == 32
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_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 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_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 test_inverse():
q1 = Quat(0.4619398, 0.1913417, 0.4619398,
0.7325378) # 45 around X then Y then Z
q2 = q1.inverse()
q_true = Quat(-0.4619398, -0.1913417, -0.4619398, 0.7325378)
assert q2 == q_true
q1.invert()
assert q1 == q_true
def test_major_axis_constructors():
q1 = Quat.from_rotation_on_axis(0, np.pi / 4)
q2 = Quat.from_rotation_on_axis(1, np.pi / 3)
q3 = Quat.from_rotation_on_axis(2, np.pi / 2)
q1_true = Quat(0.3826834, 0, 0, 0.9238795)
q2_true = Quat(0, 0.5, 0, 0.8660254)
q3_true = Quat(0, 0, 0.7071068, 0.7071068)
assert q1 == q1_true
assert q2 == q2_true
assert q3 == q3_true
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_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_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_finite():
q1 = Quat(10000000000, 210000000000, 310000000000, -2147483647)
assert q1.is_finite()
q2 = Quat(np.nan, 210000000000, 310000000000, -2147483647)
assert not q2.is_finite()
assert q2.is_nan()
q3 = Quat(np.inf, 210000000000, 310000000000, -2147483647)
assert not q3.is_finite()
assert q3.is_nan()
q4 = Quat(0.0, 210000000000, np.NINF, -2147483647)
assert not q4.is_finite()
assert q4.is_nan()
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_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_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 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 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 test_setters():
q = Quat()
q.x = 10
q.y = 40.0
q.z = 22 / 3.0
q.w = np.pi
assert q.x == 10
assert q.y == 40
assert q.z == 22 / 3.0
assert q.w == np.pi
def test_mat3_numpy():
q1_true = Quat()
m1_true = np.diag((1, 1, 1))
q1 = Quat.from_mat3_numpy(m1_true)
m1 = q1_true.to_mat3_numpy()
assert q1 == q1_true
assert np.allclose(m1, m1_true)
q2_true = Quat(0.6374259, 0.7264568, 0.204462, -0.1553838)
m2_true = np.array([
[-0.1390883, 0.9896649, 0.0348995],
[0.8625845, 0.1037672, 0.4951569],
[0.4864179, 0.0989743, -0.8681024],
])
q2 = Quat.from_mat3_numpy(m2_true)
m2 = q2_true.to_mat3_numpy()
assert q2 == q2_true
assert np.allclose(m2, m2_true)
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 decodeValue(jsonData):
"""Returns a constructed math value based on the provided json data.
Args:
jsondata (dict): The JSON data to use to decode into a Math value.
Returns:
object: The constructed math value
"""
if type(jsonData) is not dict:
return jsonData
if '__mathObjectClass__' not in jsonData:
raise Exception("Invalid JSON data for constructing value:" +
str(jsonData))
if jsonData['__mathObjectClass__'] == 'Vec2':
val = Vec2()
val.jsonDecode(jsonData, decodeValue)
elif jsonData['__mathObjectClass__'] == 'Vec3':
val = Vec3()
val.jsonDecode(jsonData, decodeValue)
elif jsonData['__mathObjectClass__'] == 'Vec4':
val = Vec4()
val.jsonDecode(jsonData, decodeValue)
elif jsonData['__mathObjectClass__'] == 'Euler':
val = Euler()
val.jsonDecode(jsonData, decodeValue)
elif jsonData['__mathObjectClass__'] == 'Quat':
val = Quat()
val.jsonDecode(jsonData, decodeValue)
elif jsonData['__mathObjectClass__'] == 'Xfo':
val = Xfo()
val.jsonDecode(jsonData, decodeValue)
elif jsonData['__mathObjectClass__'] == 'Mat33':
val = Mat33()
val.jsonDecode(jsonData, decodeValue)
elif jsonData['__mathObjectClass__'] == 'Mat44':
val = Mat44()
val.jsonDecode(jsonData, decodeValue)
else:
raise Exception("Unsupported Math type:" +
jsonData['__mathObjectClass__'])
return val
def product(qa, qb):
"""Use this product to compose the rotations represented by two quaterions.
Example:
>>> q1 = Quat()
>>> q2 = Quat()
>>> Quat.product(q1,q2)
[0.,0.,0.,1.]
"""
# Here is a readable version :
# array([ qa[3]*qb[0] + qb[3]*qa[0] + qa[1]*qb[2] - qa[2]*qb[1],
# qa[3]*qb[1] + qb[3]*qa[1] + qa[2]*qb[0] - qa[0]*qb[2],
# qa[3]*qb[2] + qb[3]*qa[2] + qa[0]*qb[1] - qa[1]*qb[0],
# qa[3]*qb[3] - qb[0]*qa[0] - qa[1]*qb[1] - qa[2]*qb[2] ])
return Quat(numpy.hstack( (qa.getRe()*qb.getIm() + qb.getRe()*qa.getIm() + numpy.cross( qa.getIm(), qb.getIm() ), [qa.getRe() * qb.getRe() - numpy.dot( qa.getIm(), qb.getIm())] )))
def getAxisAngle(self):
""" Returns rotation vector corresponding to unit quaternion in the form of [axis, angle]
"""
import sys
q = Quat(self)
q.flip() # flip q first to ensure that angle is in the [-0, pi] range
angle = 2.0 * math.acos(q.getRe())
if angle > sys.float_info.epsilon:
return [q.getIm() / math.sin(angle / 2.), angle]
norm = numpy.linalg.norm(q.getIm())
if norm > sys.float_info.epsilon:
sign = 1.0 if angle > 0 else -1.0
return [q.getIm() * (sign / norm), angle]
return [numpy.zeros(3), angle]
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
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 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_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 rotate(self, v):
"""Function rotate of class Quat rotate a vector using a quaternion.
Examples:
>>> q = [0.707, 0.0, -0.707, 0.0]
>>> v = q.rotate([1., 0., 0.])
>>> print(v)
[ 0.0, 0.0, -1.0]
"""
q = Quat(self)
q.normalize()
v0 = (1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2])) * v[0] + (
2.0 * (q[0] * q[1] - q[2] * q[3])) * v[1] + (
2.0 * (q[2] * q[0] + q[1] * q[3])) * v[2]
v1 = (2.0 * (q[0] * q[1] + q[2] * q[3])) * v[0] + (
1.0 - 2.0 * (q[2] * q[2] + q[0] * q[0])) * v[1] + (
2.0 * (q[1] * q[2] - q[0] * q[3])) * v[2]
v2 = (2.0 * (q[2] * q[0] - q[1] * q[3])) * v[0] + (
2.0 * (q[1] * q[2] + q[0] * q[3])) * v[1] + (
1.0 - 2.0 * (q[1] * q[1] + q[0] * q[0])) * v[2]
return numpy.array([v0, v1, v2])
def createFromEuler(a, axes='sxyz', inDegree=False):
"""Returns a quaternion from Euler angles (in radian) and axis sequence.
The quaternion is of type Quat.
Args:
a is a list of three Euler angles [x,y,z]
axes : One of 24 axis sequences as string or encoded tuple
Example:
>>> q = Quat.createFromEuler([-pi, 0., 0.], 'sxyz')
>>> print(q)
[ 1.0 0.0 0.0 0.0]
>>> q = Quat.createFromEuler([-pi/2., pi/2., 0.], 'ryxz') #r stands for repetition
>>> print(q)
[ 0.5 -0.5 0.5 0.5]
"""
if inDegree:
a = [a[0] * pi / 180, a[1] * pi / 180, a[2] * pi / 180]
try:
firstaxis, parity, repetition, frame = AXES_TO_TUPLE[axes.lower()]
except (AttributeError, KeyError):
TUPLE_TO_AXES[axes] # validation
firstaxis, parity, repetition, frame = axes
i = firstaxis
j = NEXT_AXIS[i + parity]
k = NEXT_AXIS[i - parity + 1]
if frame:
a[0], a[2] = a[2], a[0]
if parity:
a[1] = -a[1]
a[0] /= 2.0
a[1] /= 2.0
a[2] /= 2.0
ci = math.cos(a[0])
si = math.sin(a[0])
cj = math.cos(a[1])
sj = math.sin(a[1])
ck = math.cos(a[2])
sk = math.sin(a[2])
cc = ci * ck
cs = ci * sk
sc = si * ck
ss = si * sk
q = Quat()
if repetition:
q[3] = cj * (cc - ss)
q[i] = cj * (cs + sc)
q[j] = sj * (cc + ss)
q[k] = sj * (cs - sc)
else:
q[3] = cj * cc + sj * ss
q[i] = cj * sc - sj * cs
q[j] = cj * ss + sj * cc
q[k] = cj * cs - sj * sc
if parity:
q[j] *= -1.0
return q