1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
|
# ===-- quat.py -----------------------------------------------------------===##
#
# The KLEE Symbolic Virtual Machine
#
# This file is distributed under the University of Illinois Open Source
# License. See LICENSE.TXT for details.
#
# ===----------------------------------------------------------------------===##
from __future__ import division
import math
import vec3, vec4
def identity():
return (0.0,0.0,0.0,1.0)
def fromaxisangle(axisangle):
axis,angle= axisangle
ang_2= angle/2.0
s_ang= math.sin(ang_2)
c_ang= math.cos(ang_2)
q= vec3.mulN(axis, s_ang) + (c_ang,)
return normalize(q)
def fromnormals(n1,n2):
axis,angle= vec3.normalize(vec3.cross(n1, n2)), math.acos(vec3.dot(n1, n2))
return fromaxisangle((axis,angle))
# avoid trigonmetry
def fromnormals_faster(n1,n2):
axis= vec3.normalize(vec3.cross(n1, n2))
half_n= vec3.normalize(vec3.add(n1, n2))
cos_half_angle= vec3.dot(n1, half_n)
sin_half_angle= 1.0 - cos_half_angle**2
return vec3.mulN(axis, sin_half_angle) + (cos_half_angle,)
def fromvectors(v1,v2):
return fromnormals(vec3.normalize(v1), vec3.normalize(v2))
def magnitude(q):
return vec4.length(q)
def normalize(q):
return vec4.divN(q, magnitude(q))
def conjugate(q):
x,y,z,w= q
return (-x, -y, -z, w)
def mulvec3(q, v):
t= mul(q, v+(0.0,))
t= mul(t, conjugate(q))
return t[:3]
def mul(a, b):
ax,ay,az,aw= a
bx,by,bz,bw= b
x= aw*bx + ax*bw + ay*bz - az*by
y= aw*by + ay*bw + az*bx - ax*bz
z= aw*bz + az*bw + ax*by - ay*bx
w= aw*bw - ax*bx - ay*by - az*bz
return (x,y,z,w)
def toaxisangle(q):
tw= math.acos(q[3])
scale= math.sin(tw)
angle= tw*2.0
try:
axis= vec3.divN(q[:3], scale)
except ZeroDivisionError:
axis= (1.0,0.0,0.0)
return axis,angle
def tomat3x3(q):
x,y,z,w= q
m0= ( 1.0 - 2.0 * ( y*y + z*z ),
2.0 * ( x*y - z*w ),
2.0 * ( x*z + y*w ))
m1= ( 2.0 * ( x*y + z*w ),
1.0 - 2.0 * ( x*x + z*z ),
2.0 * ( y*z - x*w ))
m2= ( 2.0 * ( x*z - y*w ),
2.0 * ( y*z + x*w ),
1.0 - 2.0 * ( x*x + y*y ))
return m0,m1,m2
def tomat4x4(q):
m0,m1,m2= tomat3x3(q)
return (m0 + (0.0,),
m1 + (0.0,),
m2 + (0.0,),
(0.0, 0.0, 0.0, 1.0))
def slerp(a, b, t):
raise NotImplementedError
cos_omega= vec4.dot(a, b)
if (cos_omega<0.0):
cos_omega= -cos_omega
b= vec4.neg(b)
imega= math.acos(cos_omega)
t= sin(t*omega)/sin(omega)
return vec4.lerp(a, b, t)
|
