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 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 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])
