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 package ffx.numerics.math;
39
40
41
42
43
44
45
46
47
48 public final class MatrixMath {
49
50
51
52
53
54
55
56 public static double determinant3(double[][] m) {
57 return m[0][0] * m[1][1] * m[2][2]
58 - m[0][0] * m[1][2] * m[2][1]
59 + m[0][1] * m[1][2] * m[2][0]
60 - m[0][1] * m[1][0] * m[2][2]
61 + m[0][2] * m[1][0] * m[2][1]
62 - m[0][2] * m[1][1] * m[2][0];
63 }
64
65
66
67
68
69
70
71 public static double determinant3(double[] m) {
72 return m[0] * m[1] * m[2]
73 - m[0] * m[5] * m[5]
74 + m[3] * m[5] * m[4]
75 - m[3] * m[3] * m[2]
76 + m[4] * m[3] * m[5]
77 - m[4] * m[1] * m[4];
78 }
79
80
81
82
83
84
85
86 public static double[][] mat3Inverse(double[][] m) {
87 return mat3Inverse(m, new double[3][3]);
88 }
89
90
91
92
93
94
95
96
97 public static double[][] mat3Inverse(double[][] m, double[][] res) {
98 double det = determinant3(m);
99 res[0][0] = (m[1][1] * m[2][2] - m[1][2] * m[2][1]) / det;
100 res[0][1] = (m[0][2] * m[2][1] - m[0][1] * m[2][2]) / det;
101 res[0][2] = (m[0][1] * m[1][2] - m[0][2] * m[1][1]) / det;
102 res[1][0] = (m[1][2] * m[2][0] - m[1][0] * m[2][2]) / det;
103 res[1][1] = (m[0][0] * m[2][2] - m[0][2] * m[2][0]) / det;
104 res[1][2] = (m[0][2] * m[1][0] - m[0][0] * m[1][2]) / det;
105 res[2][0] = (m[1][0] * m[2][1] - m[1][1] * m[2][0]) / det;
106 res[2][1] = (m[0][1] * m[2][0] - m[0][0] * m[2][1]) / det;
107 res[2][2] = (m[0][0] * m[1][1] - m[0][1] * m[1][0]) / det;
108 return res;
109 }
110
111
112
113
114
115
116
117
118 public static double[][] mat3Mat3(double[][] m1, double[][] m2) {
119 return mat3Mat3(m1, m2, new double[3][3]);
120 }
121
122
123
124
125
126
127
128
129
130 public static double[][] mat3Mat3(double[][] m1, double[][] m2, double[][] res) {
131 res[0][0] = m1[0][0] * m2[0][0] + m1[0][1] * m2[1][0] + m1[0][2] * m2[2][0];
132 res[0][1] = m1[0][0] * m2[0][1] + m1[0][1] * m2[1][1] + m1[0][2] * m2[2][1];
133 res[0][2] = m1[0][0] * m2[0][2] + m1[0][1] * m2[1][2] + m1[0][2] * m2[2][2];
134 res[1][0] = m1[1][0] * m2[0][0] + m1[1][1] * m2[1][0] + m1[1][2] * m2[2][0];
135 res[1][1] = m1[1][0] * m2[0][1] + m1[1][1] * m2[1][1] + m1[1][2] * m2[2][1];
136 res[1][2] = m1[1][0] * m2[0][2] + m1[1][1] * m2[1][2] + m1[1][2] * m2[2][2];
137 res[2][0] = m1[2][0] * m2[0][0] + m1[2][1] * m2[1][0] + m1[2][2] * m2[2][0];
138 res[2][1] = m1[2][0] * m2[0][1] + m1[2][1] * m2[1][1] + m1[2][2] * m2[2][1];
139 res[2][2] = m1[2][0] * m2[0][2] + m1[2][1] * m2[1][2] + m1[2][2] * m2[2][2];
140 return res;
141 }
142
143
144
145
146
147
148
149
150 public static double[][] mat4Mat4(double[][] m1, double[][] m2) {
151 return mat4Mat4(m1, m2, new double[4][4]);
152 }
153
154
155
156
157
158
159
160
161
162 public static double[][] mat4Mat4(double[][] m1, double[][] m2, double[][] res) {
163 assert (m1.length == 4);
164 assert (m1[0].length == 4);
165 assert (m2.length == 4);
166 assert (m2[0].length == 4);
167 assert (res.length == 4);
168 assert (res[0].length == 4);
169 res[0][0] =
170 m1[0][0] * m2[0][0] + m1[0][1] * m2[1][0] + m1[0][2] * m2[2][0] + m1[0][3] * m2[3][0];
171 res[0][1] =
172 m1[0][0] * m2[0][1] + m1[0][1] * m2[1][1] + m1[0][2] * m2[2][1] + m1[0][3] * m2[3][1];
173 res[0][2] =
174 m1[0][0] * m2[0][2] + m1[0][1] * m2[1][2] + m1[0][2] * m2[2][2] + m1[0][3] * m2[3][2];
175 res[0][3] =
176 m1[0][0] * m2[0][3] + m1[0][1] * m2[1][3] + m1[0][2] * m2[2][3] + m1[0][3] * m2[3][3];
177 res[1][0] =
178 m1[1][0] * m2[0][0] + m1[1][1] * m2[1][0] + m1[1][2] * m2[2][0] + m1[1][3] * m2[3][0];
179 res[1][1] =
180 m1[1][0] * m2[0][1] + m1[1][1] * m2[1][1] + m1[1][2] * m2[2][1] + m1[1][3] * m2[3][1];
181 res[1][2] =
182 m1[1][0] * m2[0][2] + m1[1][1] * m2[1][2] + m1[1][2] * m2[2][2] + m1[1][3] * m2[3][2];
183 res[1][3] =
184 m1[1][0] * m2[0][3] + m1[1][1] * m2[1][3] + m1[1][2] * m2[2][3] + m1[1][3] * m2[3][3];
185 res[2][0] =
186 m1[2][0] * m2[0][0] + m1[2][1] * m2[1][0] + m1[2][2] * m2[2][0] + m1[2][3] * m2[3][0];
187 res[2][1] =
188 m1[2][0] * m2[0][1] + m1[2][1] * m2[1][1] + m1[2][2] * m2[2][1] + m1[2][3] * m2[3][1];
189 res[2][2] =
190 m1[2][0] * m2[0][2] + m1[2][1] * m2[1][2] + m1[2][2] * m2[2][2] + m1[2][3] * m2[3][2];
191 res[2][3] =
192 m1[2][0] * m2[0][3] + m1[2][1] * m2[1][3] + m1[2][2] * m2[2][3] + m1[2][3] * m2[3][3];
193 res[3][0] =
194 m1[3][0] * m2[0][0] + m1[3][1] * m2[1][0] + m1[3][2] * m2[2][0] + m1[3][3] * m2[3][0];
195 res[3][1] =
196 m1[3][0] * m2[0][1] + m1[3][1] * m2[1][1] + m1[3][2] * m2[2][1] + m1[3][3] * m2[3][1];
197 res[3][2] =
198 m1[3][0] * m2[0][2] + m1[3][1] * m2[1][2] + m1[3][2] * m2[2][2] + m1[3][3] * m2[3][2];
199 res[3][3] =
200 m1[3][0] * m2[0][3] + m1[3][1] * m2[1][3] + m1[3][2] * m2[2][3] + m1[3][3] * m2[3][3];
201 return res;
202 }
203
204
205
206
207
208
209
210
211 public static double[][] mat3SymVec6(double[][] m, double[] v) {
212 return mat3SymVec6(m, v, new double[3][3]);
213 }
214
215
216
217
218
219
220
221
222
223 public static double[][] mat3SymVec6(double[][] m, double[] v, double[][] res) {
224 res[0][0] = m[0][0] * v[0] + m[0][1] * v[3] + m[0][2] * v[4];
225 res[0][1] = m[0][0] * v[3] + m[0][1] * v[1] + m[0][2] * v[5];
226 res[0][2] = m[0][0] * v[4] + m[0][1] * v[5] + m[0][2] * v[2];
227 res[1][0] = m[1][0] * v[0] + m[1][1] * v[3] + m[1][2] * v[4];
228 res[1][1] = m[1][0] * v[3] + m[1][1] * v[1] + m[1][2] * v[5];
229 res[1][2] = m[1][0] * v[4] + m[1][1] * v[5] + m[1][2] * v[2];
230 res[2][0] = m[2][0] * v[0] + m[2][1] * v[3] + m[2][2] * v[4];
231 res[2][1] = m[2][0] * v[3] + m[2][1] * v[1] + m[2][2] * v[5];
232 res[2][2] = m[2][0] * v[4] + m[2][1] * v[5] + m[2][2] * v[2];
233 return res;
234 }
235
236
237
238
239
240
241
242
243 public static double[] mat3Vec3(double[] v, double[][] m) {
244 return mat3Vec3(v, m, new double[3]);
245 }
246
247
248
249
250
251
252
253
254
255 public static double[] mat3Vec3(double[] v, double[][] m, double[] res) {
256 res[0] = m[0][0] * v[0] + m[0][1] * v[1] + m[0][2] * v[2];
257 res[1] = m[1][0] * v[0] + m[1][1] * v[1] + m[1][2] * v[2];
258 res[2] = m[2][0] * v[0] + m[2][1] * v[1] + m[2][2] * v[2];
259 return res;
260 }
261
262
263
264
265
266
267
268
269
270 public static double[][] scalarMat3Mat3(double scalar, double[][] m1, double[][] m2) {
271 return scalarMat3Mat3(scalar, m1, m2, new double[3][3]);
272 }
273
274
275
276
277
278
279
280
281
282
283 public static double[][] scalarMat3Mat3(
284 double scalar, double[][] m1, double[][] m2, double[][] res) {
285 res[0][0] =
286 (scalar * m1[0][0]) * m2[0][0]
287 + (scalar * m1[0][1]) * m2[1][0]
288 + (scalar * m1[0][2]) * m2[2][0];
289 res[0][1] =
290 (scalar * m1[0][0]) * m2[0][1]
291 + (scalar * m1[0][1]) * m2[1][1]
292 + (scalar * m1[0][2]) * m2[2][1];
293 res[0][2] =
294 (scalar * m1[0][0]) * m2[0][2]
295 + (scalar * m1[0][1]) * m2[1][2]
296 + (scalar * m1[0][2]) * m2[2][2];
297 res[1][0] =
298 (scalar * m1[1][0]) * m2[0][0]
299 + (scalar * m1[1][1]) * m2[1][0]
300 + (scalar * m1[1][2]) * m2[2][0];
301 res[1][1] =
302 (scalar * m1[1][0]) * m2[0][1]
303 + (scalar * m1[1][1]) * m2[1][1]
304 + (scalar * m1[1][2]) * m2[2][1];
305 res[1][2] =
306 (scalar * m1[1][0]) * m2[0][2]
307 + (scalar * m1[1][1]) * m2[1][2]
308 + (scalar * m1[1][2]) * m2[2][2];
309 res[2][0] =
310 (scalar * m1[2][0]) * m2[0][0]
311 + (scalar * m1[2][1]) * m2[1][0]
312 + (scalar * m1[2][2]) * m2[2][0];
313 res[2][1] =
314 (scalar * m1[2][0]) * m2[0][1]
315 + (scalar * m1[2][1]) * m2[1][1]
316 + (scalar * m1[2][2]) * m2[2][1];
317 res[2][2] =
318 (scalar * m1[2][0]) * m2[0][2]
319 + (scalar * m1[2][1]) * m2[1][2]
320 + (scalar * m1[2][2]) * m2[2][2];
321 return res;
322 }
323
324
325
326
327
328
329
330
331 public static double[][] symVec6Mat3(double[] v, double[][] m) {
332 double[][] res = new double[3][3];
333 symVec6Mat3(v, m, res);
334 return res;
335 }
336
337
338
339
340
341
342
343
344
345 public static double[][] symVec6Mat3(double[] v, double[][] m, double[][] res) {
346 res[0][0] = v[0] * m[0][0] + v[3] * m[1][0] + v[4] * m[2][0];
347 res[0][1] = v[0] * m[0][1] + v[3] * m[1][1] + v[4] * m[2][1];
348 res[0][2] = v[0] * m[0][2] + v[3] * m[1][2] + v[4] * m[2][2];
349 res[1][0] = v[3] * m[0][0] + v[1] * m[1][0] + v[5] * m[2][0];
350 res[1][1] = v[3] * m[0][1] + v[1] * m[1][1] + v[5] * m[2][1];
351 res[1][2] = v[3] * m[0][2] + v[1] * m[1][2] + v[5] * m[2][2];
352 res[2][0] = v[4] * m[0][0] + v[5] * m[1][0] + v[2] * m[2][0];
353 res[2][1] = v[4] * m[0][1] + v[5] * m[1][1] + v[2] * m[2][1];
354 res[2][2] = v[4] * m[0][2] + v[5] * m[1][2] + v[2] * m[2][2];
355 return res;
356 }
357
358
359
360
361
362
363
364 public static double[][] transpose3(double[][] m) {
365 return transpose3(m, new double[3][3]);
366 }
367
368
369
370
371
372
373
374
375 public static double[][] transpose3(double[][] m, double[][] t) {
376 t[0][0] = m[0][0];
377 t[0][1] = m[1][0];
378 t[0][2] = m[2][0];
379 t[1][0] = m[0][1];
380 t[1][1] = m[1][1];
381 t[1][2] = m[2][1];
382 t[2][0] = m[0][2];
383 t[2][1] = m[1][2];
384 t[2][2] = m[2][2];
385 return t;
386 }
387
388
389
390
391
392
393
394
395 public static double[] vec3Mat3(double[] v, double[][] m) {
396 return vec3Mat3(v, m, new double[3]);
397 }
398
399
400
401
402
403
404
405
406
407 public static double[] vec3Mat3(double[] v, double[][] m, double[] res) {
408 res[0] = v[0] * m[0][0] + v[1] * m[1][0] + v[2] * m[2][0];
409 res[1] = v[0] * m[0][1] + v[1] * m[1][1] + v[2] * m[2][1];
410 res[2] = v[0] * m[0][2] + v[1] * m[1][2] + v[2] * m[2][2];
411 return res;
412 }
413
414
415
416
417
418
419
420
421 public static void applyMatrixTranspose(double[] in, double[] out, double[][] matrix) {
422 double xc = in[0];
423 double yc = in[1];
424 double zc = in[2];
425 out[0] = xc * matrix[0][0] + yc * matrix[1][0] + zc * matrix[2][0];
426 out[1] = xc * matrix[0][1] + yc * matrix[1][1] + zc * matrix[2][1];
427 out[2] = xc * matrix[0][2] + yc * matrix[1][2] + zc * matrix[2][2];
428 }
429 }