+def v4_dot( a, b ):
+ return a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + a[3]*a[3]
+
+def v4_length( a ):
+ return math.sqrt( v4_dot(a,a) )
+
+def m3x3_mul( a, b, d ):
+ a00 = a[0][0]
+ a01 = a[0][1]
+ a02 = a[0][2]
+ a10 = a[1][0]
+ a11 = a[1][1]
+ a12 = a[1][2]
+ a20 = a[2][0]
+ a21 = a[2][1]
+ a22 = a[2][2]
+ b00 = b[0][0]
+ b01 = b[0][1]
+ b02 = b[0][2]
+ b10 = b[1][0]
+ b11 = b[1][1]
+ b12 = b[1][2]
+ b20 = b[2][0]
+ b21 = b[2][1]
+ b22 = b[2][2]
+ d[0][0] = a00*b00 + a10*b01 + a20*b02
+ d[0][1] = a01*b00 + a11*b01 + a21*b02
+ d[0][2] = a02*b00 + a12*b01 + a22*b02
+ d[1][0] = a00*b10 + a10*b11 + a20*b12
+ d[1][1] = a01*b10 + a11*b11 + a21*b12
+ d[1][2] = a02*b10 + a12*b11 + a22*b12
+ d[2][0] = a00*b20 + a10*b21 + a20*b22
+ d[2][1] = a01*b20 + a11*b21 + a21*b22
+ d[2][2] = a02*b20 + a12*b21 + a22*b22
+
+def q_m3x3( q, d ):
+ l = v4_length(q)
+ s = 2.0 if l > 0.0 else 0.0
+ xx = s*q[0]*q[0]
+ xy = s*q[0]*q[1]
+ wx = s*q[3]*q[0]
+ yy = s*q[1]*q[1]
+ yz = s*q[1]*q[2]
+ wy = s*q[3]*q[1]
+ zz = s*q[2]*q[2]
+ xz = s*q[0]*q[2]
+ wz = s*q[3]*q[2]
+ d[0][0] = 1.0 - yy - zz
+ d[1][1] = 1.0 - xx - zz
+ d[2][2] = 1.0 - xx - yy
+ d[0][1] = xy + wz
+ d[1][2] = yz + wx
+ d[2][0] = xz + wy
+ d[1][0] = xy - wz
+ d[2][1] = yz - wx
+ d[0][2] = xz - wy
+
+def m3x3_q( m, q ):
+ diag = m[0][0] + m[1][1] + m[2][2]
+ if diag >= 0.0:
+ r = math.sqrt( 1.0 + diag )
+ rinv = 0.5 / r
+ q[0] = rinv * (m[1][2] - m[2][1])
+ q[1] = rinv * (m[2][0] - m[0][2])
+ q[2] = rinv * (m[0][1] - m[1][0])
+ q[3] = r * 0.5
+ elif m[0][0] >= m[1][1] and m[0][0] >= m[2][2]:
+ r = math.sqrt( 1.0 - m[1][1] - m[2][2] + m[0][0] )
+ rinv = 0.5 / r
+ q[0] = r * 0.5
+ q[1] = rinv * (m[0][1] + m[1][0])
+ q[2] = rinv * (m[0][2] + m[2][0])
+ q[3] = rinv * (m[1][2] - m[2][1])
+ elif m[1][1] >= m[2][2]:
+ r = math.sqrt( 1.0 - m[0][0] - m[2][2] + m[1][1] )
+ rinv = 0.5 / r
+ q[0] = rinv * (m[0][1] + m[1][0])
+ q[1] = r * 0.5
+ q[2] = rinv * (m[1][2] + m[2][1])
+ q[3] = rinv * (m[2][0] - m[0][2])
+ else:
+ r = math.sqrt( 1.0 - m[0][0] - m[1][1] + m[2][2] )
+ rinv = 0.5 / r
+ q[0] = rinv * (m[0][2] + m[2][0])
+ q[1] = rinv * (m[1][2] + m[2][1])
+ q[2] = r * 0.5
+ q[3] = rinv * (m[0][1] - m[1][0])