-
Notifications
You must be signed in to change notification settings - Fork 0
/
quat.go
executable file
·134 lines (112 loc) · 2.93 KB
/
quat.go
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
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
package gglm
import (
"fmt"
)
var _ Swizzle4 = &Quat{}
var _ fmt.Stringer = &Quat{}
type Quat struct {
Vec4
}
// Eq checks for exact equality
func (q *Quat) Eq(q2 *Quat) bool {
return q.Data == q2.Data
}
// Angle returns the angle represented by this quaternion in radians
func (q *Quat) Angle() float32 {
if Abs32(q.Data[3]) > CosHalf {
a := Asin32(Sqrt32(q.Data[0]*q.Data[0]+q.Data[1]*q.Data[1]+q.Data[2]*q.Data[2])) * 2
if q.Data[3] < 0 {
return Pi*2 - a
}
return a
}
return Acos32(q.Data[3]) * 2
}
// Axis returns the rotation axis represented by this quaternion
func (q *Quat) Axis() Vec3 {
var t float32 = 1 - q.Data[3]*q.Data[3]
if t <= 0 {
return Vec3{Data: [3]float32{0, 0, 1}}
}
t = 1 / Sqrt32(t)
return Vec3{Data: [3]float32{
q.Data[0] * t,
q.Data[1] * t,
q.Data[2] * t,
}}
}
// NewQuatEulerVec takes rotations in radians and produces a rotation that
// rotates around the z-axis, y-axis and lastly x-axis.
func NewQuatEulerVec(v *Vec3) Quat {
return NewQuatEuler(v.X(), v.Y(), v.Z())
}
// NewQuatEuler takes rotations in radians and produces a rotation that
// rotates around the z-axis, y-axis and lastly x-axis.
func NewQuatEuler(x, y, z float32) Quat {
//Some other common terminology: x=roll, y=pitch, z=yaw
sinX, cosX := Sincos32(x * 0.5)
sinY, cosY := Sincos32(y * 0.5)
sinZ, cosZ := Sincos32(z * 0.5)
//This produces a z->y->x multiply order, but its written as XYZ.
//This is due to XYZ meaning independent rotation matrices, so Z is applied
//first, then Y matrix and lastly X.
//See this for more info: https://github.com/godotengine/godot/issues/6816#issuecomment-254592170
//
//Note: On most conversion tools putting the multiply order (e.g. ZYX for us) is required.
return Quat{
Vec4: Vec4{
Data: [4]float32{
sinX*cosY*cosZ - cosX*sinY*sinZ,
cosX*sinY*cosZ + sinX*cosY*sinZ,
cosX*cosY*sinZ - sinX*sinY*cosZ,
cosX*cosY*cosZ + sinX*sinY*sinZ,
},
},
}
}
// NewQuatAngleAxisVec produces a quaternion thats rotates rotRad radians around the *normalized* vector rotAxisNorm
func NewQuatAngleAxisVec(rotRad float32, rotAxisNorm *Vec3) Quat {
return NewQuatAngleAxis(rotRad, rotAxisNorm.X(), rotAxisNorm.Y(), rotAxisNorm.Z())
}
// NewQuatAngleAxis produces a quaternion thats rotates rotRad radians around the *normalized* vector rotAxisNorm
func NewQuatAngleAxis(rotRad float32, rotAxisNormX, rotAxisNormY, rotAxisNormZ float32) Quat {
s, c := Sincos32(rotRad * 0.5)
return Quat{
Vec4: Vec4{
Data: [4]float32{
rotAxisNormX * s,
rotAxisNormY * s,
rotAxisNormZ * s,
c,
},
},
}
}
func NewQuatId() Quat {
return Quat{
Vec4: Vec4{
Data: [4]float32{0, 0, 0, 1},
},
}
}
func NewQuat(x, y, z, w float32) Quat {
return Quat{
Vec4: Vec4{
Data: [4]float32{x, y, z, w},
},
}
}
func NewQuatArr(arr [4]float32) Quat {
return Quat{
Vec4: Vec4{
Data: arr,
},
}
}
func NewQuatVec(v *Vec4) Quat {
return Quat{
Vec4: Vec4{
Data: v.Data,
},
}
}