Source code for optimeed.visualize.gui.widgets.openGLWidget.quaternions

from math import *
import numpy


[docs]def normalize(v, tolerance=0.001): mag2 = sum(n * n for n in v) if abs(mag2 - 1.0) > tolerance: mag = sqrt(mag2) v = tuple(n / mag for n in v) return v
[docs]def q_mult(q1, q2): w1, x1, y1, z1 = q1 w2, x2, y2, z2 = q2 w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2 x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2 y = w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2 z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2 return w, x, y, z
[docs]def q_conjugate(q): w, x, y, z = q return w, -x, -y, -z
[docs]def qv_mult(q1, v1): q2 = (0.0,) + v1 return q_mult(q_mult(q1, q2), q_conjugate(q1))[1:]
[docs]def axisangle_to_q(v, theta): v = normalize(v) x, y, z = v theta /= 2 w = cos(theta) x = x * sin(theta) y = y * sin(theta) z = z * sin(theta) return w, x, y, z
[docs]def q_to_axisangle(q): w, v = q[0], q[1:] theta = acos(w) * 2.0 return normalize(v), theta
[docs]def q_to_mat4(q): w, x, y, z = q return numpy.array([[1 - 2*y*y - 2*z*z, 2*x*y - 2*z*w, 2*x*z + 2*y*w, 0], [2*x*y + 2*z*w, 1 - 2*x*x - 2*z*z, 2*y*z - 2*x*w, 0], [2*x*z - 2*y*w, 2*y*z + 2*x*w, 1 - 2*x*x - 2*y*y, 0], [0, 0, 0, 1]], 'f')