-
Notifications
You must be signed in to change notification settings - Fork 0
/
geom-quat.stanza
96 lines (74 loc) · 2.83 KB
/
geom-quat.stanza
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
defpackage geom/quat :
import core
import math
import geom/angle
import geom/vec
import geom/mat
val @doc-quat = "QUAT -- quaternions for interpolatable rotations"
public defstruct Quat <: Equalable :
w: Double
v: Vec3
public defn quat-from-euler (radz:Vec3) -> Quat :
val [sr cr] = sin-cos-tup(0.5 * x(radz))
val [sp cp] = sin-cos-tup(0.5 * y(radz))
val [sy cy] = sin-cos-tup(0.5 * z(radz))
Quat( cy * cr * cp + sy * sr * sp,
Vec3(cy * sr * cp - sy * cr * sp,
cy * cr * sp + sy * sr * cp,
sy * cr * cp - cy * sr * sp))
public defn quat-from-axis (axis:Vec3, rads:Double) -> Quat :
val angle = rads / 2.0
Quat(cos(angle), sin(angle) * normalize(axis))
val zero-quat = quat-from-axis(Vec3(1.0, 0.0, 0.0), 0.0)
public defn zero? (q:Quat) : q == zero-quat
public defn Quat () : zero-quat
defmethod print (o:OutputStream, q:Quat) :
print-all(o, ["Quat(" v(q) "," w(q) ")"])
defmethod equal? (q1:Quat, q2:Quat) :
v(q1) == v(q2) and w(q1) == w(q2)
public defn times (s:Double, q:Quat) -> Quat :
Quat(s * w(q), s * v(q))
public defn divide (q:Quat, d:Double) -> Quat :
Quat(w(q) / d, v(q) / d)
public defn x (q:Quat) -> Double : x(v(q))
public defn y (q:Quat) -> Double : y(v(q))
public defn z (q:Quat) -> Double : z(v(q))
public defn times (q1:Quat, q2:Quat) -> Quat :
Quat(w(q1) * w(q2) - dot(v(q1), v(q2)), w(q1) * v(q2) + w(q2) * v(q1) + v(q1) % v(q2))
public defn dot (q1:Quat, q2:Quat) -> Double :
dot(v(q1), v(q2)) + w(q1) * w(q2)
public defn conjugate (q:Quat) -> Quat :
Quat(w(q), (- v(q)))
public defn negate (q:Quat) -> Quat :
conjugate(q) / dot(q, q)
public defn magnitude (q:Quat) -> Double :
sqrt(dot(q, q))
public defn normalize (q:Quat) -> Quat :
q / magnitude(q)
val DELTA = 0.95
public defn slerp (src:Quat, dst0:Quat, t:Double) -> Quat :
val cosom0 = dot(src, dst0)
val [cosom, dst] =
if cosom0 < 0.0 :
[(- cosom0), -1.0 * dst0]
else :
[cosom0, dst0]
val [s0, s1] =
if (1.0 - cosom) > DELTA :
val omega = acos(cosom)
val sinom = sin(omega)
[sin((1.0 - t) * omega) / sinom, sin(t * omega) / sinom]
else :
[1.0 - t, t]
Quat(s0 * w(src) + s1 * w(dst), s0 * v(src) + s1 * v(dst))
public defn quat-to-mat4 (q:Quat) -> Mat4 :
val [ wx, wy, wz] = [w(q) * x(q), w(q) * y(q), w(q) * z(q)]
val [ xx, xy, xz] = [x(q) * x(q), x(q) * y(q), x(q) * z(q)]
val [ yy, yz, zz] = [y(q) * y(q), y(q) * z(q), z(q) * z(q)]
val [m00, m01, m02] = [1.0 - 2.0 * (yy + zz), 2.0 * (xy - wz), 2.0 * (xz + wy)]
val [m10, m11, m12] = [ 2.0 * (xy + wz), 1.0 - 2.0 * (xx + zz), 2.0 * (yz - wx)]
val [m20, m21, m22] = [ 2.0 * (xz - wy), 2.0 * (yz + wx), 1.0 - 2.0 * (xx + yy)]
Mat4( m00, m01, m02, 0.0,
m10, m11, m12, 0.0,
m20, m21, m22, 0.0,
0.0, 0.0, 0.0, 1.0)