-
Notifications
You must be signed in to change notification settings - Fork 2
/
mat4.go
320 lines (288 loc) · 10.6 KB
/
mat4.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
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
package gm
import (
"bytes"
"fmt"
"text/tabwriter"
)
// Mat4 is a 4x4 matrix in row major order.
//
// m[4*r + c] is the element in the r'th row and c'th column.
type Mat4 [16]Float
// Col returns a vector representing the corresponding column (starting at col 0).
// This package makes no distinction between row and column vectors, so it
// will be a normal VecN for a MxN matrix.
func (m Mat4) Col(n int) Vec4 {
return Vec4{m[n*4+0], m[n*4+1], m[n*4+2], m[n*4+3]}
}
// Abs returns the element-wise absolute value of this matrix
func (m Mat4) Abs() Mat4 {
return Mat4{
Abs(m[0]), Abs(m[1]), Abs(m[2]), Abs(m[3]),
Abs(m[4]), Abs(m[5]), Abs(m[6]), Abs(m[7]),
Abs(m[8]), Abs(m[9]), Abs(m[10]), Abs(m[11]),
Abs(m[12]), Abs(m[13]), Abs(m[14]), Abs(m[15]),
}
}
// Add performs an element-wise addition of two matrices, this is
// equivalent to iterating over every element of m1 and adding the corresponding value of m2.
func (m Mat4) Add(m2 Mat4) Mat4 {
return Mat4{
m[0] + m2[0], m[1] + m2[1], m[2] + m2[2], m[3] + m2[3],
m[4] + m2[4], m[5] + m2[5], m[6] + m2[6], m[7] + m2[7],
m[8] + m2[8], m[9] + m2[9], m[10] + m2[10], m[11] + m2[11],
m[12] + m2[12], m[13] + m2[13], m[14] + m2[14], m[15] + m2[15],
}
}
// MulS performs a scalar multiplcation of the matrix. This is equivalent to iterating
// over every element of the matrix and multiply it by c.
func (m Mat4) MulS(v Float) Mat4 {
return Mat4{
m[0] * v, m[1] * v, m[2] * v, m[3] * v,
m[4] * v, m[5] * v, m[6] * v, m[7] * v,
m[8] * v, m[9] * v, m[10] * v, m[11] * v,
m[12] * v, m[13] * v, m[14] * v, m[15] * v,
}
}
// Mul performs a "matrix product" between this matrix
// and another of the given dimension. For any two matrices of dimensionality
// MxN and NxO, the result will be MxO. For instance, Mat4 multiplied using
// Mul4x2 will result in a Mat4x2.
func (m Mat4) Mul(m2 Mat4) Mat4 {
return Mat4{
m[0]*m2[0] + m[4]*m2[1] + m[8]*m2[2] + m[12]*m2[3],
m[1]*m2[0] + m[5]*m2[1] + m[9]*m2[2] + m[13]*m2[3],
m[2]*m2[0] + m[6]*m2[1] + m[10]*m2[2] + m[14]*m2[3],
m[3]*m2[0] + m[7]*m2[1] + m[11]*m2[2] + m[15]*m2[3],
m[0]*m2[4] + m[4]*m2[5] + m[8]*m2[6] + m[12]*m2[7],
m[1]*m2[4] + m[5]*m2[5] + m[9]*m2[6] + m[13]*m2[7],
m[2]*m2[4] + m[6]*m2[5] + m[10]*m2[6] + m[14]*m2[7],
m[3]*m2[4] + m[7]*m2[5] + m[11]*m2[6] + m[15]*m2[7],
m[0]*m2[8] + m[4]*m2[9] + m[8]*m2[10] + m[12]*m2[11],
m[1]*m2[8] + m[5]*m2[9] + m[9]*m2[10] + m[13]*m2[11],
m[2]*m2[8] + m[6]*m2[9] + m[10]*m2[10] + m[14]*m2[11],
m[3]*m2[8] + m[7]*m2[9] + m[11]*m2[10] + m[15]*m2[11],
m[0]*m2[12] + m[4]*m2[13] + m[8]*m2[14] + m[12]*m2[15],
m[1]*m2[12] + m[5]*m2[13] + m[9]*m2[14] + m[13]*m2[15],
m[2]*m2[12] + m[6]*m2[13] + m[10]*m2[14] + m[14]*m2[15],
m[3]*m2[12] + m[7]*m2[13] + m[11]*m2[14] + m[15]*m2[15],
}
}
// MulV4 returns a Vec4 based on the multiplication of m and m2.
func (m Mat4) MulV4(m2 Vec4) Vec4 {
return Vec4{
m[0]*m2[0] + m[4]*m2[1] + m[8]*m2[2] + m[12]*m2[3],
m[1]*m2[0] + m[5]*m2[1] + m[9]*m2[2] + m[13]*m2[3],
m[2]*m2[0] + m[6]*m2[1] + m[10]*m2[2] + m[14]*m2[3],
m[3]*m2[0] + m[7]*m2[1] + m[11]*m2[2] + m[15]*m2[3],
}
}
// Det returns the determinant of a matrix. It is a measure of a square matrix's
// singularity and invertability, among other things. In this library, the
// determinant is hard coded based on pre-computed cofactor expansion, and uses
// no loops. Of course, the addition and multiplication must still be done.
func (m Mat4) Det() Float {
return m[0]*m[5]*m[10]*m[15] -
m[0]*m[5]*m[11]*m[14] -
m[0]*m[6]*m[9]*m[15] +
m[0]*m[6]*m[11]*m[13] +
m[0]*m[7]*m[9]*m[14] -
m[0]*m[7]*m[10]*m[13] -
m[1]*m[4]*m[10]*m[15] +
m[1]*m[4]*m[11]*m[14] +
m[1]*m[6]*m[8]*m[15] -
m[1]*m[6]*m[11]*m[12] -
m[1]*m[7]*m[8]*m[14] +
m[1]*m[7]*m[10]*m[12] +
m[2]*m[4]*m[9]*m[15] -
m[2]*m[4]*m[11]*m[13] -
m[2]*m[5]*m[8]*m[15] +
m[2]*m[5]*m[11]*m[12] +
m[2]*m[7]*m[8]*m[13] -
m[2]*m[7]*m[9]*m[12] -
m[3]*m[4]*m[9]*m[14] +
m[3]*m[4]*m[10]*m[13] +
m[3]*m[5]*m[8]*m[14] -
m[3]*m[5]*m[10]*m[12] -
m[3]*m[6]*m[8]*m[13] +
m[3]*m[6]*m[9]*m[12]
}
// Inv computes the inverse of a square matrix. An inverse is a square matrix
// such that when multiplied by the original, yields the identity.
//
// M_inv * M = M * M_inv = I
//
// In this library, the math is precomputed, and uses no loops, though the
// multiplications, additions, determinant calculation, and scaling are still
// done. This can still be (relatively) expensive for a 4x4.
//
// This function checks the determinant to see if the matrix is invertible.
// If the determinant is 0.0, this function returns the zero matrix. However, due to floating point errors, it is
// entirely plausible to get a false positive or negative.
// In the future, an alternate function may be written which takes in a pre-computed determinant.
func (m Mat4) Inv() Mat4 {
det := m.Det()
if FloatEqual(det, Float(0.0)) {
return Mat4{}
}
retMat := Mat4{
-m[7]*m[10]*m[13] + m[6]*m[11]*m[13] + m[7]*m[9]*m[14] - m[5]*m[11]*m[14] - m[6]*m[9]*m[15] + m[5]*m[10]*m[15],
m[3]*m[10]*m[13] - m[2]*m[11]*m[13] - m[3]*m[9]*m[14] + m[1]*m[11]*m[14] + m[2]*m[9]*m[15] - m[1]*m[10]*m[15],
-m[3]*m[6]*m[13] + m[2]*m[7]*m[13] + m[3]*m[5]*m[14] - m[1]*m[7]*m[14] - m[2]*m[5]*m[15] + m[1]*m[6]*m[15],
m[3]*m[6]*m[9] - m[2]*m[7]*m[9] - m[3]*m[5]*m[10] + m[1]*m[7]*m[10] + m[2]*m[5]*m[11] - m[1]*m[6]*m[11],
m[7]*m[10]*m[12] - m[6]*m[11]*m[12] - m[7]*m[8]*m[14] + m[4]*m[11]*m[14] + m[6]*m[8]*m[15] - m[4]*m[10]*m[15],
-m[3]*m[10]*m[12] + m[2]*m[11]*m[12] + m[3]*m[8]*m[14] - m[0]*m[11]*m[14] - m[2]*m[8]*m[15] + m[0]*m[10]*m[15],
m[3]*m[6]*m[12] - m[2]*m[7]*m[12] - m[3]*m[4]*m[14] + m[0]*m[7]*m[14] + m[2]*m[4]*m[15] - m[0]*m[6]*m[15],
-m[3]*m[6]*m[8] + m[2]*m[7]*m[8] + m[3]*m[4]*m[10] - m[0]*m[7]*m[10] - m[2]*m[4]*m[11] + m[0]*m[6]*m[11],
-m[7]*m[9]*m[12] + m[5]*m[11]*m[12] + m[7]*m[8]*m[13] - m[4]*m[11]*m[13] - m[5]*m[8]*m[15] + m[4]*m[9]*m[15],
m[3]*m[9]*m[12] - m[1]*m[11]*m[12] - m[3]*m[8]*m[13] + m[0]*m[11]*m[13] + m[1]*m[8]*m[15] - m[0]*m[9]*m[15],
-m[3]*m[5]*m[12] + m[1]*m[7]*m[12] + m[3]*m[4]*m[13] - m[0]*m[7]*m[13] - m[1]*m[4]*m[15] + m[0]*m[5]*m[15],
m[3]*m[5]*m[8] - m[1]*m[7]*m[8] - m[3]*m[4]*m[9] + m[0]*m[7]*m[9] + m[1]*m[4]*m[11] - m[0]*m[5]*m[11],
m[6]*m[9]*m[12] - m[5]*m[10]*m[12] - m[6]*m[8]*m[13] + m[4]*m[10]*m[13] + m[5]*m[8]*m[14] - m[4]*m[9]*m[14],
-m[2]*m[9]*m[12] + m[1]*m[10]*m[12] + m[2]*m[8]*m[13] - m[0]*m[10]*m[13] - m[1]*m[8]*m[14] + m[0]*m[9]*m[14],
m[2]*m[5]*m[12] - m[1]*m[6]*m[12] - m[2]*m[4]*m[13] + m[0]*m[6]*m[13] + m[1]*m[4]*m[14] - m[0]*m[5]*m[14],
-m[2]*m[5]*m[8] + m[1]*m[6]*m[8] + m[2]*m[4]*m[9] - m[0]*m[6]*m[9] - m[1]*m[4]*m[10] + m[0]*m[5]*m[10],
}
return retMat.MulS(1 / det)
}
// Transpose produces the transpose of this matrix. For any MxN matrix
// the transpose is an NxM matrix with the rows swapped with the columns. For instance
// the transpose of the Mat3x2 is a Mat2x3 like so:
//
// [[a b]] [[a c e]]
// [[c d]] = [[b d f]]
// [[e f]]
func (m Mat4) Transpose() Mat4 {
return Mat4{
m[0], m[4], m[8], m[12],
m[1], m[5], m[9], m[13],
m[2], m[6], m[10], m[14],
m[3], m[7], m[11], m[15],
}
}
// Mat3 returns a truncated mat3 removing last col and row.
func (m Mat4) Mat3() Mat3 {
return Mat3{
m[0], m[1], m[2],
m[4], m[5], m[6],
m[8], m[9], m[10],
}
}
// Diag is a basic operation on a square matrix that simply
// returns main diagonal (meaning all elements such that row==col).
func (m Mat4) Diag() Vec4 {
return Vec4{m[0], m[5], m[10], m[15]}
}
// Sub performs an element-wise subtraction of two matrices, this is equivalent
// to iterating over every element of m1 and subtracting the corresponding
// value of m2.
func (m Mat4) Sub(m2 Mat4) Mat4 {
return Mat4{
m[0] - m2[0], m[1] - m2[1], m[2] - m2[2], m[3] - m2[3],
m[4] - m2[4], m[5] - m2[5], m[6] - m2[6], m[7] - m2[7],
m[8] - m2[8], m[9] - m2[9], m[10] - m2[10], m[11] - m2[11],
m[12] - m2[12], m[13] - m2[13], m[14] - m2[14], m[15] - m2[15],
}
}
// Quat returns the quaternion representation of this matrix.
func (m Mat4) Quat() Quat {
return QLookAt(m.Col(2).Vec3(), m.Col(1).Vec3())
}
// ApproxEqual performs an element-wise approximate equality test between two
// matrices, as if FloatEqual had been used.
func (m Mat4) ApproxEqual(m2 Mat4) bool {
for i := range m {
if !FloatEqual(m[i], m2[i]) {
return false
}
}
return true
}
// ApproxEqualThreshold performs an element-wise approximate equality test
// between two matrices with a given epsilon threshold, as if
// FloatEqualThreshold had been used.
func (m Mat4) ApproxEqualThreshold(m2 Mat4, threshold Float) bool {
for i := range m {
if !FloatEqualThreshold(m[i], m2[i], threshold) {
return false
}
}
return true
}
// ApproxFuncEqual performs an element-wise approximate equality test between
// two matrices with a given equality functions, intended to be used with
// FloatEqualFunc; although and comparison
// function may be used in practice.
func (m Mat4) ApproxFuncEqual(m2 Mat4, eq func(Float, Float) bool) bool {
for i := range m {
if !eq(m[i], m2[i]) {
return false
}
}
return true
}
// Row returns a vector representing the corresponding row (starting at row 0).
// This package makes no distinction between row and column vectors, so it
// will be a normal VecM for a MxN matrix.
func (m Mat4) Row(row int) Vec4 {
return Vec4{m[row+0], m[row+4], m[row+8], m[row+12]}
}
// Trace is a basic operation on a square matrix that simply
// sums up all elements on the main diagonal (meaning all elements such that row==col).
func (m Mat4) Trace() Float {
return m[0] + m[5] + m[10] + m[15]
}
func (m Mat4) String() string {
buf := new(bytes.Buffer)
w := tabwriter.NewWriter(buf, 4, 4, 1, ' ', tabwriter.AlignRight)
for i := 0; i < 4; i++ {
for _, col := range m.Row(i) {
fmt.Fprintf(w, "%f\t", col)
}
fmt.Fprintln(w, "")
}
w.Flush() // nolint: errcheck
return buf.String()
}
// M4Ident returns a mat4 identity.
func M4Ident() Mat4 {
return Mat4{1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1}
}
// M4Mul multiply sequence of matrices
func M4Mul(ms ...Mat4) Mat4 {
r := ms[0]
for _, m := range ms[1:] {
r = r.Mul(m)
}
return r
}
// Translate3D returns a translation matrix
func Translate3D(x, y, z Float) Mat4 {
return Mat4{
1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
x, y, z, 1,
}
}
// Scale3D returns a scale matrix
func Scale3D(sx, sy, sz Float) Mat4 {
return Mat4{
sx, 0, 0, 0,
0, sy, 0, 0,
0, 0, sz, 0,
0, 0, 0, 1,
}
}
// LookAt based on f32
func LookAt(eye, center, up Vec3) Mat4 {
f := center.Sub(eye).Normalize()
s := f.Cross(up.Normalize()).Normalize()
u := s.Cross(f)
m := Mat4{
s[0], u[0], -f[0], 0,
s[1], u[1], -f[1], 0,
s[2], u[2], -f[2], 0,
0, 0, 0, 1,
}
return m.Mul(Translate3D(-eye[0], -eye[1], -eye[2]))
}