Mango request: added an input node to use track's position in compositor
[blender.git] / source / blender / blenlib / intern / math_matrix.c
1 /*
2  * ***** BEGIN GPL LICENSE BLOCK *****
3  *
4  * This program is free software; you can redistribute it and/or
5  * modify it under the terms of the GNU General Public License
6  * as published by the Free Software Foundation; either version 2
7  * of the License, or (at your option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12  * GNU General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License
15  * along with this program; if not, write to the Free Software Foundation,
16  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
17  *
18  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
19  * All rights reserved.
20  *
21  * The Original Code is: some of this file.
22  *
23  * ***** END GPL LICENSE BLOCK *****
24  */
25
26 /** \file blender/blenlib/intern/math_matrix.c
27  *  \ingroup bli
28  */
29
30
31 #include <assert.h>
32 #include "BLI_math.h"
33
34 /********************************* Init **************************************/
35
36 void zero_m3(float m[3][3])
37 {
38         memset(m, 0, 3 * 3 * sizeof(float));
39 }
40
41 void zero_m4(float m[4][4])
42 {
43         memset(m, 0, 4 * 4 * sizeof(float));
44 }
45
46 void unit_m3(float m[][3])
47 {
48         m[0][0] = m[1][1] = m[2][2] = 1.0;
49         m[0][1] = m[0][2] = 0.0;
50         m[1][0] = m[1][2] = 0.0;
51         m[2][0] = m[2][1] = 0.0;
52 }
53
54 void unit_m4(float m[][4])
55 {
56         m[0][0] = m[1][1] = m[2][2] = m[3][3] = 1.0;
57         m[0][1] = m[0][2] = m[0][3] = 0.0;
58         m[1][0] = m[1][2] = m[1][3] = 0.0;
59         m[2][0] = m[2][1] = m[2][3] = 0.0;
60         m[3][0] = m[3][1] = m[3][2] = 0.0;
61 }
62
63 void copy_m3_m3(float m1[][3], float m2[][3])
64 {
65         /* destination comes first: */
66         memcpy(&m1[0], &m2[0], 9 * sizeof(float));
67 }
68
69 void copy_m4_m4(float m1[][4], float m2[][4])
70 {
71         memcpy(m1, m2, 4 * 4 * sizeof(float));
72 }
73
74 void copy_m3_m4(float m1[][3], float m2[][4])
75 {
76         m1[0][0] = m2[0][0];
77         m1[0][1] = m2[0][1];
78         m1[0][2] = m2[0][2];
79
80         m1[1][0] = m2[1][0];
81         m1[1][1] = m2[1][1];
82         m1[1][2] = m2[1][2];
83
84         m1[2][0] = m2[2][0];
85         m1[2][1] = m2[2][1];
86         m1[2][2] = m2[2][2];
87 }
88
89 void copy_m4_m3(float m1[][4], float m2[][3]) /* no clear */
90 {
91         m1[0][0] = m2[0][0];
92         m1[0][1] = m2[0][1];
93         m1[0][2] = m2[0][2];
94
95         m1[1][0] = m2[1][0];
96         m1[1][1] = m2[1][1];
97         m1[1][2] = m2[1][2];
98
99         m1[2][0] = m2[2][0];
100         m1[2][1] = m2[2][1];
101         m1[2][2] = m2[2][2];
102
103         /*      Reevan's Bugfix */
104         m1[0][3] = 0.0F;
105         m1[1][3] = 0.0F;
106         m1[2][3] = 0.0F;
107
108         m1[3][0] = 0.0F;
109         m1[3][1] = 0.0F;
110         m1[3][2] = 0.0F;
111         m1[3][3] = 1.0F;
112
113 }
114
115 void swap_m3m3(float m1[][3], float m2[][3])
116 {
117         float t;
118         int i, j;
119
120         for (i = 0; i < 3; i++) {
121                 for (j = 0; j < 3; j++) {
122                         t = m1[i][j];
123                         m1[i][j] = m2[i][j];
124                         m2[i][j] = t;
125                 }
126         }
127 }
128
129 void swap_m4m4(float m1[][4], float m2[][4])
130 {
131         float t;
132         int i, j;
133
134         for (i = 0; i < 4; i++) {
135                 for (j = 0; j < 4; j++) {
136                         t = m1[i][j];
137                         m1[i][j] = m2[i][j];
138                         m2[i][j] = t;
139                 }
140         }
141 }
142
143 /******************************** Arithmetic *********************************/
144
145 void mult_m4_m4m4(float m1[][4], float m3_[][4], float m2_[][4])
146 {
147         float m2[4][4], m3[4][4];
148
149         /* copy so it works when m1 is the same pointer as m2 or m3 */
150         copy_m4_m4(m2, m2_);
151         copy_m4_m4(m3, m3_);
152
153         /* matrix product: m1[j][k] = m2[j][i].m3[i][k] */
154         m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] + m2[0][2] * m3[2][0] + m2[0][3] * m3[3][0];
155         m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] + m2[0][2] * m3[2][1] + m2[0][3] * m3[3][1];
156         m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] + m2[0][2] * m3[2][2] + m2[0][3] * m3[3][2];
157         m1[0][3] = m2[0][0] * m3[0][3] + m2[0][1] * m3[1][3] + m2[0][2] * m3[2][3] + m2[0][3] * m3[3][3];
158
159         m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] + m2[1][2] * m3[2][0] + m2[1][3] * m3[3][0];
160         m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] + m2[1][2] * m3[2][1] + m2[1][3] * m3[3][1];
161         m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] + m2[1][2] * m3[2][2] + m2[1][3] * m3[3][2];
162         m1[1][3] = m2[1][0] * m3[0][3] + m2[1][1] * m3[1][3] + m2[1][2] * m3[2][3] + m2[1][3] * m3[3][3];
163
164         m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] + m2[2][2] * m3[2][0] + m2[2][3] * m3[3][0];
165         m1[2][1] = m2[2][0] * m3[0][1] + m2[2][1] * m3[1][1] + m2[2][2] * m3[2][1] + m2[2][3] * m3[3][1];
166         m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] + m2[2][2] * m3[2][2] + m2[2][3] * m3[3][2];
167         m1[2][3] = m2[2][0] * m3[0][3] + m2[2][1] * m3[1][3] + m2[2][2] * m3[2][3] + m2[2][3] * m3[3][3];
168
169         m1[3][0] = m2[3][0] * m3[0][0] + m2[3][1] * m3[1][0] + m2[3][2] * m3[2][0] + m2[3][3] * m3[3][0];
170         m1[3][1] = m2[3][0] * m3[0][1] + m2[3][1] * m3[1][1] + m2[3][2] * m3[2][1] + m2[3][3] * m3[3][1];
171         m1[3][2] = m2[3][0] * m3[0][2] + m2[3][1] * m3[1][2] + m2[3][2] * m3[2][2] + m2[3][3] * m3[3][2];
172         m1[3][3] = m2[3][0] * m3[0][3] + m2[3][1] * m3[1][3] + m2[3][2] * m3[2][3] + m2[3][3] * m3[3][3];
173
174 }
175
176 void mul_m3_m3m3(float m1[][3], float m3_[][3], float m2_[][3])
177 {
178         float m2[3][3], m3[3][3];
179
180         /* copy so it works when m1 is the same pointer as m2 or m3 */
181         copy_m3_m3(m2, m2_);
182         copy_m3_m3(m3, m3_);
183
184         /* m1[i][j] = m2[i][k] * m3[k][j], args are flipped!  */
185         m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] + m2[0][2] * m3[2][0];
186         m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] + m2[0][2] * m3[2][1];
187         m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] + m2[0][2] * m3[2][2];
188
189         m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] + m2[1][2] * m3[2][0];
190         m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] + m2[1][2] * m3[2][1];
191         m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] + m2[1][2] * m3[2][2];
192
193         m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] + m2[2][2] * m3[2][0];
194         m1[2][1] = m2[2][0] * m3[0][1] + m2[2][1] * m3[1][1] + m2[2][2] * m3[2][1];
195         m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] + m2[2][2] * m3[2][2];
196 }
197
198 void mul_m4_m4m3(float (*m1)[4], float (*m3_)[4], float (*m2_)[3])
199 {
200         float m2[3][3], m3[4][4];
201
202         /* copy so it works when m1 is the same pointer as m2 or m3 */
203         copy_m3_m3(m2, m2_);
204         copy_m4_m4(m3, m3_);
205
206         m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] + m2[0][2] * m3[2][0];
207         m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] + m2[0][2] * m3[2][1];
208         m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] + m2[0][2] * m3[2][2];
209         m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] + m2[1][2] * m3[2][0];
210         m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] + m2[1][2] * m3[2][1];
211         m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] + m2[1][2] * m3[2][2];
212         m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] + m2[2][2] * m3[2][0];
213         m1[2][1] = m2[2][0] * m3[0][1] + m2[2][1] * m3[1][1] + m2[2][2] * m3[2][1];
214         m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] + m2[2][2] * m3[2][2];
215 }
216
217 /* m1 = m2 * m3, ignore the elements on the 4th row/column of m3 */
218 void mult_m3_m3m4(float m1[][3], float m3[][4], float m2[][3])
219 {
220         /* m1[i][j] = m2[i][k] * m3[k][j] */
221         m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] + m2[0][2] * m3[2][0];
222         m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] + m2[0][2] * m3[2][1];
223         m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] + m2[0][2] * m3[2][2];
224
225         m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] + m2[1][2] * m3[2][0];
226         m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] + m2[1][2] * m3[2][1];
227         m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] + m2[1][2] * m3[2][2];
228
229         m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] + m2[2][2] * m3[2][0];
230         m1[2][1] = m2[2][0] * m3[0][1] + m2[2][1] * m3[1][1] + m2[2][2] * m3[2][1];
231         m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] + m2[2][2] * m3[2][2];
232 }
233
234 void mul_m4_m3m4(float (*m1)[4], float (*m3)[3], float (*m2)[4])
235 {
236         m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] + m2[0][2] * m3[2][0];
237         m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] + m2[0][2] * m3[2][1];
238         m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] + m2[0][2] * m3[2][2];
239         m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] + m2[1][2] * m3[2][0];
240         m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] + m2[1][2] * m3[2][1];
241         m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] + m2[1][2] * m3[2][2];
242         m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] + m2[2][2] * m3[2][0];
243         m1[2][1] = m2[2][0] * m3[0][1] + m2[2][1] * m3[1][1] + m2[2][2] * m3[2][1];
244         m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] + m2[2][2] * m3[2][2];
245 }
246
247 void mul_serie_m3(float answ[][3],
248                   float m1[][3], float m2[][3], float m3[][3],
249                   float m4[][3], float m5[][3], float m6[][3],
250                   float m7[][3], float m8[][3])
251 {
252         float temp[3][3];
253
254         if (m1 == NULL || m2 == NULL) return;
255
256         mul_m3_m3m3(answ, m2, m1);
257         if (m3) {
258                 mul_m3_m3m3(temp, m3, answ);
259                 if (m4) {
260                         mul_m3_m3m3(answ, m4, temp);
261                         if (m5) {
262                                 mul_m3_m3m3(temp, m5, answ);
263                                 if (m6) {
264                                         mul_m3_m3m3(answ, m6, temp);
265                                         if (m7) {
266                                                 mul_m3_m3m3(temp, m7, answ);
267                                                 if (m8) {
268                                                         mul_m3_m3m3(answ, m8, temp);
269                                                 }
270                                                 else copy_m3_m3(answ, temp);
271                                         }
272                                 }
273                                 else copy_m3_m3(answ, temp);
274                         }
275                 }
276                 else copy_m3_m3(answ, temp);
277         }
278 }
279
280 void mul_serie_m4(float answ[][4], float m1[][4],
281                   float m2[][4], float m3[][4], float m4[][4],
282                   float m5[][4], float m6[][4], float m7[][4],
283                   float m8[][4])
284 {
285         float temp[4][4];
286
287         if (m1 == NULL || m2 == NULL) return;
288
289         mult_m4_m4m4(answ, m1, m2);
290         if (m3) {
291                 mult_m4_m4m4(temp, answ, m3);
292                 if (m4) {
293                         mult_m4_m4m4(answ, temp, m4);
294                         if (m5) {
295                                 mult_m4_m4m4(temp, answ, m5);
296                                 if (m6) {
297                                         mult_m4_m4m4(answ, temp, m6);
298                                         if (m7) {
299                                                 mult_m4_m4m4(temp, answ, m7);
300                                                 if (m8) {
301                                                         mult_m4_m4m4(answ, temp, m8);
302                                                 }
303                                                 else copy_m4_m4(answ, temp);
304                                         }
305                                 }
306                                 else copy_m4_m4(answ, temp);
307                         }
308                 }
309                 else copy_m4_m4(answ, temp);
310         }
311 }
312
313 void mul_m4_v3(float mat[][4], float vec[3])
314 {
315         float x, y;
316
317         x = vec[0];
318         y = vec[1];
319         vec[0] = x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2] + mat[3][0];
320         vec[1] = x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2] + mat[3][1];
321         vec[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2] + mat[3][2];
322 }
323
324 void mul_v3_m4v3(float in[3], float mat[][4], const float vec[3])
325 {
326         float x, y;
327
328         x = vec[0];
329         y = vec[1];
330         in[0] = x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2] + mat[3][0];
331         in[1] = x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2] + mat[3][1];
332         in[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2] + mat[3][2];
333 }
334
335 /* same as mul_m4_v3() but doesnt apply translation component */
336 void mul_mat3_m4_v3(float mat[][4], float vec[3])
337 {
338         float x, y;
339
340         x = vec[0];
341         y = vec[1];
342         vec[0] = x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2];
343         vec[1] = x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2];
344         vec[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2];
345 }
346
347 void mul_project_m4_v3(float mat[][4], float vec[3])
348 {
349         const float w = vec[0] * mat[0][3] + vec[1] * mat[1][3] + vec[2] * mat[2][3] + mat[3][3];
350         mul_m4_v3(mat, vec);
351
352         vec[0] /= w;
353         vec[1] /= w;
354         vec[2] /= w;
355 }
356
357 void mul_v4_m4v4(float r[4], float mat[4][4], float v[4])
358 {
359         float x, y, z;
360
361         x = v[0];
362         y = v[1];
363         z = v[2];
364
365         r[0] = x * mat[0][0] + y * mat[1][0] + z * mat[2][0] + mat[3][0] * v[3];
366         r[1] = x * mat[0][1] + y * mat[1][1] + z * mat[2][1] + mat[3][1] * v[3];
367         r[2] = x * mat[0][2] + y * mat[1][2] + z * mat[2][2] + mat[3][2] * v[3];
368         r[3] = x * mat[0][3] + y * mat[1][3] + z * mat[2][3] + mat[3][3] * v[3];
369 }
370
371 void mul_m4_v4(float mat[4][4], float r[4])
372 {
373         mul_v4_m4v4(r, mat, r);
374 }
375
376 void mul_v4d_m4v4d(double r[4], float mat[4][4], double v[4])
377 {
378         double x, y, z;
379
380         x = v[0];
381         y = v[1];
382         z = v[2];
383
384         r[0] = x * (double)mat[0][0] + y * (double)mat[1][0] + z * (double)mat[2][0] + (double)mat[3][0] * v[3];
385         r[1] = x * (double)mat[0][1] + y * (double)mat[1][1] + z * (double)mat[2][1] + (double)mat[3][1] * v[3];
386         r[2] = x * (double)mat[0][2] + y * (double)mat[1][2] + z * (double)mat[2][2] + (double)mat[3][2] * v[3];
387         r[3] = x * (double)mat[0][3] + y * (double)mat[1][3] + z * (double)mat[2][3] + (double)mat[3][3] * v[3];
388 }
389
390 void mul_m4_v4d(float mat[4][4], double r[4])
391 {
392         mul_v4d_m4v4d(r, mat, r);
393 }
394
395 void mul_v3_m3v3(float r[3], float M[3][3], float a[3])
396 {
397         r[0] = M[0][0] * a[0] + M[1][0] * a[1] + M[2][0] * a[2];
398         r[1] = M[0][1] * a[0] + M[1][1] * a[1] + M[2][1] * a[2];
399         r[2] = M[0][2] * a[0] + M[1][2] * a[1] + M[2][2] * a[2];
400 }
401
402 void mul_m3_v3(float M[3][3], float r[3])
403 {
404         float tmp[3];
405
406         mul_v3_m3v3(tmp, M, r);
407         copy_v3_v3(r, tmp);
408 }
409
410 void mul_transposed_m3_v3(float mat[][3], float vec[3])
411 {
412         float x, y;
413
414         x = vec[0];
415         y = vec[1];
416         vec[0] = x * mat[0][0] + y * mat[0][1] + mat[0][2] * vec[2];
417         vec[1] = x * mat[1][0] + y * mat[1][1] + mat[1][2] * vec[2];
418         vec[2] = x * mat[2][0] + y * mat[2][1] + mat[2][2] * vec[2];
419 }
420
421 void mul_m3_fl(float m[3][3], float f)
422 {
423         int i, j;
424
425         for (i = 0; i < 3; i++)
426                 for (j = 0; j < 3; j++)
427                         m[i][j] *= f;
428 }
429
430 void mul_m4_fl(float m[4][4], float f)
431 {
432         int i, j;
433
434         for (i = 0; i < 4; i++)
435                 for (j = 0; j < 4; j++)
436                         m[i][j] *= f;
437 }
438
439 void mul_mat3_m4_fl(float m[4][4], float f)
440 {
441         int i, j;
442
443         for (i = 0; i < 3; i++)
444                 for (j = 0; j < 3; j++)
445                         m[i][j] *= f;
446 }
447
448 void mul_m3_v3_double(float mat[][3], double vec[3])
449 {
450         double x, y;
451
452         x = vec[0];
453         y = vec[1];
454         vec[0] = x * (double)mat[0][0] + y * (double)mat[1][0] + (double)mat[2][0] * vec[2];
455         vec[1] = x * (double)mat[0][1] + y * (double)mat[1][1] + (double)mat[2][1] * vec[2];
456         vec[2] = x * (double)mat[0][2] + y * (double)mat[1][2] + (double)mat[2][2] * vec[2];
457 }
458
459 void add_m3_m3m3(float m1[][3], float m2[][3], float m3[][3])
460 {
461         int i, j;
462
463         for (i = 0; i < 3; i++)
464                 for (j = 0; j < 3; j++)
465                         m1[i][j] = m2[i][j] + m3[i][j];
466 }
467
468 void add_m4_m4m4(float m1[][4], float m2[][4], float m3[][4])
469 {
470         int i, j;
471
472         for (i = 0; i < 4; i++)
473                 for (j = 0; j < 4; j++)
474                         m1[i][j] = m2[i][j] + m3[i][j];
475 }
476
477 void sub_m3_m3m3(float m1[][3], float m2[][3], float m3[][3])
478 {
479         int i, j;
480
481         for (i = 0; i < 3; i++)
482                 for (j = 0; j < 3; j++)
483                         m1[i][j] = m2[i][j] - m3[i][j];
484 }
485
486 void sub_m4_m4m4(float m1[][4], float m2[][4], float m3[][4])
487 {
488         int i, j;
489
490         for (i = 0; i < 4; i++)
491                 for (j = 0; j < 4; j++)
492                         m1[i][j] = m2[i][j] - m3[i][j];
493 }
494
495 int invert_m3(float m[3][3])
496 {
497         float tmp[3][3];
498         int success;
499
500         success = invert_m3_m3(tmp, m);
501         copy_m3_m3(m, tmp);
502
503         return success;
504 }
505
506 int invert_m3_m3(float m1[3][3], float m2[3][3])
507 {
508         float det;
509         int a, b, success;
510
511         /* calc adjoint */
512         adjoint_m3_m3(m1, m2);
513
514         /* then determinant old matrix! */
515         det = (m2[0][0] * (m2[1][1] * m2[2][2] - m2[1][2] * m2[2][1]) -
516                m2[1][0] * (m2[0][1] * m2[2][2] - m2[0][2] * m2[2][1]) +
517                m2[2][0] * (m2[0][1] * m2[1][2] - m2[0][2] * m2[1][1]));
518
519         success = (det != 0);
520
521         if (det == 0) det = 1;
522         det = 1 / det;
523         for (a = 0; a < 3; a++) {
524                 for (b = 0; b < 3; b++) {
525                         m1[a][b] *= det;
526                 }
527         }
528
529         return success;
530 }
531
532 int invert_m4(float m[4][4])
533 {
534         float tmp[4][4];
535         int success;
536
537         success = invert_m4_m4(tmp, m);
538         copy_m4_m4(m, tmp);
539
540         return success;
541 }
542
543 /*
544  * invertmat -
545  *      computes the inverse of mat and puts it in inverse.  Returns
546  *  TRUE on success (i.e. can always find a pivot) and FALSE on failure.
547  *  Uses Gaussian Elimination with partial (maximal column) pivoting.
548  *
549  *                                      Mark Segal - 1992
550  */
551
552 int invert_m4_m4(float inverse[4][4], float mat[4][4])
553 {
554         int i, j, k;
555         double temp;
556         float tempmat[4][4];
557         float max;
558         int maxj;
559
560         /* Set inverse to identity */
561         for (i = 0; i < 4; i++)
562                 for (j = 0; j < 4; j++)
563                         inverse[i][j] = 0;
564         for (i = 0; i < 4; i++)
565                 inverse[i][i] = 1;
566
567         /* Copy original matrix so we don't mess it up */
568         for (i = 0; i < 4; i++)
569                 for (j = 0; j < 4; j++)
570                         tempmat[i][j] = mat[i][j];
571
572         for (i = 0; i < 4; i++) {
573                 /* Look for row with max pivot */
574                 max = fabs(tempmat[i][i]);
575                 maxj = i;
576                 for (j = i + 1; j < 4; j++) {
577                         if (fabsf(tempmat[j][i]) > max) {
578                                 max = fabs(tempmat[j][i]);
579                                 maxj = j;
580                         }
581                 }
582                 /* Swap rows if necessary */
583                 if (maxj != i) {
584                         for (k = 0; k < 4; k++) {
585                                 SWAP(float, tempmat[i][k], tempmat[maxj][k]);
586                                 SWAP(float, inverse[i][k], inverse[maxj][k]);
587                         }
588                 }
589
590                 temp = tempmat[i][i];
591                 if (temp == 0)
592                         return 0;  /* No non-zero pivot */
593                 for (k = 0; k < 4; k++) {
594                         tempmat[i][k] = (float)(tempmat[i][k] / temp);
595                         inverse[i][k] = (float)(inverse[i][k] / temp);
596                 }
597                 for (j = 0; j < 4; j++) {
598                         if (j != i) {
599                                 temp = tempmat[j][i];
600                                 for (k = 0; k < 4; k++) {
601                                         tempmat[j][k] -= (float)(tempmat[i][k] * temp);
602                                         inverse[j][k] -= (float)(inverse[i][k] * temp);
603                                 }
604                         }
605                 }
606         }
607         return 1;
608 }
609
610 /****************************** Linear Algebra *******************************/
611
612 void transpose_m3(float mat[][3])
613 {
614         float t;
615
616         t = mat[0][1];
617         mat[0][1] = mat[1][0];
618         mat[1][0] = t;
619         t = mat[0][2];
620         mat[0][2] = mat[2][0];
621         mat[2][0] = t;
622         t = mat[1][2];
623         mat[1][2] = mat[2][1];
624         mat[2][1] = t;
625 }
626
627 void transpose_m4(float mat[][4])
628 {
629         float t;
630
631         t = mat[0][1];
632         mat[0][1] = mat[1][0];
633         mat[1][0] = t;
634         t = mat[0][2];
635         mat[0][2] = mat[2][0];
636         mat[2][0] = t;
637         t = mat[0][3];
638         mat[0][3] = mat[3][0];
639         mat[3][0] = t;
640
641         t = mat[1][2];
642         mat[1][2] = mat[2][1];
643         mat[2][1] = t;
644         t = mat[1][3];
645         mat[1][3] = mat[3][1];
646         mat[3][1] = t;
647
648         t = mat[2][3];
649         mat[2][3] = mat[3][2];
650         mat[3][2] = t;
651 }
652
653 void orthogonalize_m3(float mat[][3], int axis)
654 {
655         float size[3];
656         mat3_to_size(size, mat);
657         normalize_v3(mat[axis]);
658         switch (axis) {
659                 case 0:
660                         if (dot_v3v3(mat[0], mat[1]) < 1) {
661                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
662                                 normalize_v3(mat[2]);
663                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
664                         }
665                         else if (dot_v3v3(mat[0], mat[2]) < 1) {
666                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
667                                 normalize_v3(mat[1]);
668                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
669                         }
670                         else {
671                                 float vec[3];
672
673                                 vec[0] = mat[0][1];
674                                 vec[1] = mat[0][2];
675                                 vec[2] = mat[0][0];
676
677                                 cross_v3_v3v3(mat[2], mat[0], vec);
678                                 normalize_v3(mat[2]);
679                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
680                         }
681                 case 1:
682                         if (dot_v3v3(mat[1], mat[0]) < 1) {
683                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
684                                 normalize_v3(mat[2]);
685                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
686                         }
687                         else if (dot_v3v3(mat[0], mat[2]) < 1) {
688                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
689                                 normalize_v3(mat[0]);
690                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
691                         }
692                         else {
693                                 float vec[3];
694
695                                 vec[0] = mat[1][1];
696                                 vec[1] = mat[1][2];
697                                 vec[2] = mat[1][0];
698
699                                 cross_v3_v3v3(mat[0], mat[1], vec);
700                                 normalize_v3(mat[0]);
701                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
702                         }
703                 case 2:
704                         if (dot_v3v3(mat[2], mat[0]) < 1) {
705                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
706                                 normalize_v3(mat[1]);
707                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
708                         }
709                         else if (dot_v3v3(mat[2], mat[1]) < 1) {
710                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
711                                 normalize_v3(mat[0]);
712                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
713                         }
714                         else {
715                                 float vec[3];
716
717                                 vec[0] = mat[2][1];
718                                 vec[1] = mat[2][2];
719                                 vec[2] = mat[2][0];
720
721                                 cross_v3_v3v3(mat[0], vec, mat[2]);
722                                 normalize_v3(mat[0]);
723                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
724                         }
725         }
726         mul_v3_fl(mat[0], size[0]);
727         mul_v3_fl(mat[1], size[1]);
728         mul_v3_fl(mat[2], size[2]);
729 }
730
731 void orthogonalize_m4(float mat[][4], int axis)
732 {
733         float size[3];
734         mat4_to_size(size, mat);
735         normalize_v3(mat[axis]);
736         switch (axis) {
737                 case 0:
738                         if (dot_v3v3(mat[0], mat[1]) < 1) {
739                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
740                                 normalize_v3(mat[2]);
741                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
742                         }
743                         else if (dot_v3v3(mat[0], mat[2]) < 1) {
744                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
745                                 normalize_v3(mat[1]);
746                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
747                         }
748                         else {
749                                 float vec[3];
750
751                                 vec[0] = mat[0][1];
752                                 vec[1] = mat[0][2];
753                                 vec[2] = mat[0][0];
754
755                                 cross_v3_v3v3(mat[2], mat[0], vec);
756                                 normalize_v3(mat[2]);
757                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
758                         }
759                 case 1:
760                         normalize_v3(mat[0]);
761                         if (dot_v3v3(mat[1], mat[0]) < 1) {
762                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
763                                 normalize_v3(mat[2]);
764                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
765                         }
766                         else if (dot_v3v3(mat[0], mat[2]) < 1) {
767                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
768                                 normalize_v3(mat[0]);
769                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
770                         }
771                         else {
772                                 float vec[3];
773
774                                 vec[0] = mat[1][1];
775                                 vec[1] = mat[1][2];
776                                 vec[2] = mat[1][0];
777
778                                 cross_v3_v3v3(mat[0], mat[1], vec);
779                                 normalize_v3(mat[0]);
780                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
781                         }
782                 case 2:
783                         if (dot_v3v3(mat[2], mat[0]) < 1) {
784                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
785                                 normalize_v3(mat[1]);
786                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
787                         }
788                         else if (dot_v3v3(mat[2], mat[1]) < 1) {
789                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
790                                 normalize_v3(mat[0]);
791                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
792                         }
793                         else {
794                                 float vec[3];
795
796                                 vec[0] = mat[2][1];
797                                 vec[1] = mat[2][2];
798                                 vec[2] = mat[2][0];
799
800                                 cross_v3_v3v3(mat[0], vec, mat[2]);
801                                 normalize_v3(mat[0]);
802                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
803                         }
804         }
805         mul_v3_fl(mat[0], size[0]);
806         mul_v3_fl(mat[1], size[1]);
807         mul_v3_fl(mat[2], size[2]);
808 }
809
810 int is_orthogonal_m3(float m[][3])
811 {
812         int i, j;
813
814         for (i = 0; i < 3; i++) {
815                 for (j = 0; j < i; j++) {
816                         if (fabsf(dot_v3v3(m[i], m[j])) > 1.5f * FLT_EPSILON)
817                                 return 0;
818                 }
819         }
820
821         return 1;
822 }
823
824 int is_orthogonal_m4(float m[][4])
825 {
826         int i, j;
827
828         for (i = 0; i < 4; i++) {
829                 for (j = 0; j < i; j++) {
830                         if (fabsf(dot_vn_vn(m[i], m[j], 4)) > 1.5f * FLT_EPSILON)
831                                 return 0;
832                 }
833
834         }
835
836         return 1;
837 }
838
839 int is_orthonormal_m3(float m[][3])
840 {
841         if (is_orthogonal_m3(m)) {
842                 int i;
843
844                 for (i = 0; i < 3; i++)
845                         if (fabsf(dot_v3v3(m[i], m[i]) - 1) > 1.5f * FLT_EPSILON)
846                                 return 0;
847
848                 return 1;
849         }
850
851         return 0;
852 }
853
854 int is_orthonormal_m4(float m[][4])
855 {
856         if (is_orthogonal_m4(m)) {
857                 int i;
858
859                 for (i = 0; i < 4; i++)
860                         if (fabsf(dot_vn_vn(m[i], m[i], 4) - 1) > 1.5f * FLT_EPSILON)
861                                 return 0;
862
863                 return 1;
864         }
865
866         return 0;
867 }
868
869 int is_uniform_scaled_m3(float m[][3])
870 {
871         const float eps = 1e-7;
872         float t[3][3];
873         float l1, l2, l3, l4, l5, l6;
874
875         copy_m3_m3(t, m);
876         transpose_m3(t);
877
878         l1 = len_squared_v3(m[0]);
879         l2 = len_squared_v3(m[1]);
880         l3 = len_squared_v3(m[2]);
881
882         l4 = len_squared_v3(t[0]);
883         l5 = len_squared_v3(t[1]);
884         l6 = len_squared_v3(t[2]);
885
886         if (fabsf(l2 - l1) <= eps &&
887             fabsf(l3 - l1) <= eps &&
888             fabsf(l4 - l1) <= eps &&
889             fabsf(l5 - l1) <= eps &&
890             fabsf(l6 - l1) <= eps)
891         {
892                 return 1;
893         }
894
895         return 0;
896 }
897
898 void normalize_m3(float mat[][3])
899 {
900         normalize_v3(mat[0]);
901         normalize_v3(mat[1]);
902         normalize_v3(mat[2]);
903 }
904
905 void normalize_m3_m3(float rmat[][3], float mat[][3])
906 {
907         normalize_v3_v3(rmat[0], mat[0]);
908         normalize_v3_v3(rmat[1], mat[1]);
909         normalize_v3_v3(rmat[2], mat[2]);
910 }
911
912 void normalize_m4(float mat[][4])
913 {
914         float len;
915
916         len = normalize_v3(mat[0]);
917         if (len != 0.0f) mat[0][3] /= len;
918         len = normalize_v3(mat[1]);
919         if (len != 0.0f) mat[1][3] /= len;
920         len = normalize_v3(mat[2]);
921         if (len != 0.0f) mat[2][3] /= len;
922 }
923
924 void normalize_m4_m4(float rmat[][4], float mat[][4])
925 {
926         float len;
927
928         len = normalize_v3_v3(rmat[0], mat[0]);
929         if (len != 0.0f) rmat[0][3] = mat[0][3] / len;
930         len = normalize_v3_v3(rmat[1], mat[1]);
931         if (len != 0.0f) rmat[1][3] = mat[1][3] / len;
932         len = normalize_v3_v3(rmat[2], mat[2]);
933         if (len != 0.0f) rmat[2][3] = mat[2][3] / len;
934 }
935
936 void adjoint_m3_m3(float m1[][3], float m[][3])
937 {
938         m1[0][0] = m[1][1] * m[2][2] - m[1][2] * m[2][1];
939         m1[0][1] = -m[0][1] * m[2][2] + m[0][2] * m[2][1];
940         m1[0][2] = m[0][1] * m[1][2] - m[0][2] * m[1][1];
941
942         m1[1][0] = -m[1][0] * m[2][2] + m[1][2] * m[2][0];
943         m1[1][1] = m[0][0] * m[2][2] - m[0][2] * m[2][0];
944         m1[1][2] = -m[0][0] * m[1][2] + m[0][2] * m[1][0];
945
946         m1[2][0] = m[1][0] * m[2][1] - m[1][1] * m[2][0];
947         m1[2][1] = -m[0][0] * m[2][1] + m[0][1] * m[2][0];
948         m1[2][2] = m[0][0] * m[1][1] - m[0][1] * m[1][0];
949 }
950
951 void adjoint_m4_m4(float out[][4], float in[][4]) /* out = ADJ(in) */
952 {
953         float a1, a2, a3, a4, b1, b2, b3, b4;
954         float c1, c2, c3, c4, d1, d2, d3, d4;
955
956         a1 = in[0][0];
957         b1 = in[0][1];
958         c1 = in[0][2];
959         d1 = in[0][3];
960
961         a2 = in[1][0];
962         b2 = in[1][1];
963         c2 = in[1][2];
964         d2 = in[1][3];
965
966         a3 = in[2][0];
967         b3 = in[2][1];
968         c3 = in[2][2];
969         d3 = in[2][3];
970
971         a4 = in[3][0];
972         b4 = in[3][1];
973         c4 = in[3][2];
974         d4 = in[3][3];
975
976
977         out[0][0] = determinant_m3(b2, b3, b4, c2, c3, c4, d2, d3, d4);
978         out[1][0] = -determinant_m3(a2, a3, a4, c2, c3, c4, d2, d3, d4);
979         out[2][0] = determinant_m3(a2, a3, a4, b2, b3, b4, d2, d3, d4);
980         out[3][0] = -determinant_m3(a2, a3, a4, b2, b3, b4, c2, c3, c4);
981
982         out[0][1] = -determinant_m3(b1, b3, b4, c1, c3, c4, d1, d3, d4);
983         out[1][1] = determinant_m3(a1, a3, a4, c1, c3, c4, d1, d3, d4);
984         out[2][1] = -determinant_m3(a1, a3, a4, b1, b3, b4, d1, d3, d4);
985         out[3][1] = determinant_m3(a1, a3, a4, b1, b3, b4, c1, c3, c4);
986
987         out[0][2] = determinant_m3(b1, b2, b4, c1, c2, c4, d1, d2, d4);
988         out[1][2] = -determinant_m3(a1, a2, a4, c1, c2, c4, d1, d2, d4);
989         out[2][2] = determinant_m3(a1, a2, a4, b1, b2, b4, d1, d2, d4);
990         out[3][2] = -determinant_m3(a1, a2, a4, b1, b2, b4, c1, c2, c4);
991
992         out[0][3] = -determinant_m3(b1, b2, b3, c1, c2, c3, d1, d2, d3);
993         out[1][3] = determinant_m3(a1, a2, a3, c1, c2, c3, d1, d2, d3);
994         out[2][3] = -determinant_m3(a1, a2, a3, b1, b2, b3, d1, d2, d3);
995         out[3][3] = determinant_m3(a1, a2, a3, b1, b2, b3, c1, c2, c3);
996 }
997
998 float determinant_m2(float a, float b, float c, float d)
999 {
1000
1001         return a * d - b * c;
1002 }
1003
1004 float determinant_m3(float a1, float a2, float a3,
1005                      float b1, float b2, float b3,
1006                      float c1, float c2, float c3)
1007 {
1008         float ans;
1009
1010         ans = (a1 * determinant_m2(b2, b3, c2, c3) -
1011                b1 * determinant_m2(a2, a3, c2, c3) +
1012                c1 * determinant_m2(a2, a3, b2, b3));
1013
1014         return ans;
1015 }
1016
1017 float determinant_m4(float m[][4])
1018 {
1019         float ans;
1020         float a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4;
1021
1022         a1 = m[0][0];
1023         b1 = m[0][1];
1024         c1 = m[0][2];
1025         d1 = m[0][3];
1026
1027         a2 = m[1][0];
1028         b2 = m[1][1];
1029         c2 = m[1][2];
1030         d2 = m[1][3];
1031
1032         a3 = m[2][0];
1033         b3 = m[2][1];
1034         c3 = m[2][2];
1035         d3 = m[2][3];
1036
1037         a4 = m[3][0];
1038         b4 = m[3][1];
1039         c4 = m[3][2];
1040         d4 = m[3][3];
1041
1042         ans = (a1 * determinant_m3(b2, b3, b4, c2, c3, c4, d2, d3, d4) -
1043                b1 * determinant_m3(a2, a3, a4, c2, c3, c4, d2, d3, d4) +
1044                c1 * determinant_m3(a2, a3, a4, b2, b3, b4, d2, d3, d4) -
1045                d1 * determinant_m3(a2, a3, a4, b2, b3, b4, c2, c3, c4));
1046
1047         return ans;
1048 }
1049
1050 /****************************** Transformations ******************************/
1051
1052 void size_to_mat3(float mat[][3], const float size[3])
1053 {
1054         mat[0][0] = size[0];
1055         mat[0][1] = 0.0f;
1056         mat[0][2] = 0.0f;
1057         mat[1][1] = size[1];
1058         mat[1][0] = 0.0f;
1059         mat[1][2] = 0.0f;
1060         mat[2][2] = size[2];
1061         mat[2][1] = 0.0f;
1062         mat[2][0] = 0.0f;
1063 }
1064
1065 void size_to_mat4(float mat[][4], const float size[3])
1066 {
1067         float tmat[3][3];
1068
1069         size_to_mat3(tmat, size);
1070         unit_m4(mat);
1071         copy_m4_m3(mat, tmat);
1072 }
1073
1074 void mat3_to_size(float size[3], float mat[][3])
1075 {
1076         size[0] = len_v3(mat[0]);
1077         size[1] = len_v3(mat[1]);
1078         size[2] = len_v3(mat[2]);
1079 }
1080
1081 void mat4_to_size(float size[3], float mat[][4])
1082 {
1083         size[0] = len_v3(mat[0]);
1084         size[1] = len_v3(mat[1]);
1085         size[2] = len_v3(mat[2]);
1086 }
1087
1088 /* this gets the average scale of a matrix, only use when your scaling
1089  * data that has no idea of scale axis, examples are bone-envelope-radius
1090  * and curve radius */
1091 float mat3_to_scale(float mat[][3])
1092 {
1093         /* unit length vector */
1094         float unit_vec[3] = {0.577350269189626f, 0.577350269189626f, 0.577350269189626f};
1095         mul_m3_v3(mat, unit_vec);
1096         return len_v3(unit_vec);
1097 }
1098
1099 float mat4_to_scale(float mat[][4])
1100 {
1101         float tmat[3][3];
1102         copy_m3_m4(tmat, mat);
1103         return mat3_to_scale(tmat);
1104 }
1105
1106 void mat3_to_rot_size(float rot[3][3], float size[3], float mat3[3][3])
1107 {
1108         float mat3_n[3][3]; /* mat3 -> normalized, 3x3 */
1109         float imat3_n[3][3]; /* mat3 -> normalized & inverted, 3x3 */
1110
1111         /* rotation & scale are linked, we need to create the mat's
1112          * for these together since they are related. */
1113
1114         /* so scale doesnt interfear with rotation [#24291] */
1115         /* note: this is a workaround for negative matrix not working for rotation conversion, FIXME */
1116         normalize_m3_m3(mat3_n, mat3);
1117         if (is_negative_m3(mat3)) {
1118                 negate_v3(mat3_n[0]);
1119                 negate_v3(mat3_n[1]);
1120                 negate_v3(mat3_n[2]);
1121         }
1122
1123         /* rotation */
1124         /* keep rot as a 3x3 matrix, the caller can convert into a quat or euler */
1125         copy_m3_m3(rot, mat3_n);
1126
1127         /* scale */
1128         /* note: mat4_to_size(ob->size, mat) fails for negative scale */
1129         invert_m3_m3(imat3_n, mat3_n);
1130         mul_m3_m3m3(mat3, imat3_n, mat3);
1131
1132         size[0] = mat3[0][0];
1133         size[1] = mat3[1][1];
1134         size[2] = mat3[2][2];
1135 }
1136
1137 void mat4_to_loc_rot_size(float loc[3], float rot[3][3], float size[3], float wmat[][4])
1138 {
1139         float mat3[3][3]; /* wmat -> 3x3 */
1140
1141         copy_m3_m4(mat3, wmat);
1142         mat3_to_rot_size(rot, size, mat3);
1143
1144         /* location */
1145         copy_v3_v3(loc, wmat[3]);
1146 }
1147
1148 void scale_m3_fl(float m[][3], float scale)
1149 {
1150         m[0][0] = m[1][1] = m[2][2] = scale;
1151         m[0][1] = m[0][2] = 0.0;
1152         m[1][0] = m[1][2] = 0.0;
1153         m[2][0] = m[2][1] = 0.0;
1154 }
1155
1156 void scale_m4_fl(float m[][4], float scale)
1157 {
1158         m[0][0] = m[1][1] = m[2][2] = scale;
1159         m[3][3] = 1.0;
1160         m[0][1] = m[0][2] = m[0][3] = 0.0;
1161         m[1][0] = m[1][2] = m[1][3] = 0.0;
1162         m[2][0] = m[2][1] = m[2][3] = 0.0;
1163         m[3][0] = m[3][1] = m[3][2] = 0.0;
1164 }
1165
1166 void translate_m4(float mat[][4], float Tx, float Ty, float Tz)
1167 {
1168         mat[3][0] += (Tx * mat[0][0] + Ty * mat[1][0] + Tz * mat[2][0]);
1169         mat[3][1] += (Tx * mat[0][1] + Ty * mat[1][1] + Tz * mat[2][1]);
1170         mat[3][2] += (Tx * mat[0][2] + Ty * mat[1][2] + Tz * mat[2][2]);
1171 }
1172
1173 void rotate_m4(float mat[][4], const char axis, const float angle)
1174 {
1175         int col;
1176         float temp[4] = {0.0f, 0.0f, 0.0f, 0.0f};
1177         float cosine, sine;
1178
1179         assert(axis >= 'X' && axis <= 'Z');
1180
1181         cosine = (float)cos(angle);
1182         sine = (float)sin(angle);
1183         switch (axis) {
1184                 case 'X':
1185                         for (col = 0; col < 4; col++)
1186                                 temp[col] = cosine * mat[1][col] + sine * mat[2][col];
1187                         for (col = 0; col < 4; col++) {
1188                                 mat[2][col] = -sine * mat[1][col] + cosine * mat[2][col];
1189                                 mat[1][col] = temp[col];
1190                         }
1191                         break;
1192
1193                 case 'Y':
1194                         for (col = 0; col < 4; col++)
1195                                 temp[col] = cosine * mat[0][col] - sine * mat[2][col];
1196                         for (col = 0; col < 4; col++) {
1197                                 mat[2][col] = sine * mat[0][col] + cosine * mat[2][col];
1198                                 mat[0][col] = temp[col];
1199                         }
1200                         break;
1201
1202                 case 'Z':
1203                         for (col = 0; col < 4; col++)
1204                                 temp[col] = cosine * mat[0][col] + sine * mat[1][col];
1205                         for (col = 0; col < 4; col++) {
1206                                 mat[1][col] = -sine * mat[0][col] + cosine * mat[1][col];
1207                                 mat[0][col] = temp[col];
1208                         }
1209                         break;
1210         }
1211 }
1212
1213 void blend_m3_m3m3(float out[][3], float dst[][3], float src[][3], const float srcweight)
1214 {
1215         float srot[3][3], drot[3][3];
1216         float squat[4], dquat[4], fquat[4];
1217         float sscale[3], dscale[3], fsize[3];
1218         float rmat[3][3], smat[3][3];
1219
1220         mat3_to_rot_size(drot, dscale, dst);
1221         mat3_to_rot_size(srot, sscale, src);
1222
1223         mat3_to_quat(dquat, drot);
1224         mat3_to_quat(squat, srot);
1225
1226         /* do blending */
1227         interp_qt_qtqt(fquat, dquat, squat, srcweight);
1228         interp_v3_v3v3(fsize, dscale, sscale, srcweight);
1229
1230         /* compose new matrix */
1231         quat_to_mat3(rmat, fquat);
1232         size_to_mat3(smat, fsize);
1233         mul_m3_m3m3(out, rmat, smat);
1234 }
1235
1236 void blend_m4_m4m4(float out[][4], float dst[][4], float src[][4], const float srcweight)
1237 {
1238         float sloc[3], dloc[3], floc[3];
1239         float srot[3][3], drot[3][3];
1240         float squat[4], dquat[4], fquat[4];
1241         float sscale[3], dscale[3], fsize[3];
1242
1243         mat4_to_loc_rot_size(dloc, drot, dscale, dst);
1244         mat4_to_loc_rot_size(sloc, srot, sscale, src);
1245
1246         mat3_to_quat(dquat, drot);
1247         mat3_to_quat(squat, srot);
1248
1249         /* do blending */
1250         interp_v3_v3v3(floc, dloc, sloc, srcweight);
1251         interp_qt_qtqt(fquat, dquat, squat, srcweight);
1252         interp_v3_v3v3(fsize, dscale, sscale, srcweight);
1253
1254         /* compose new matrix */
1255         loc_quat_size_to_mat4(out, floc, fquat, fsize);
1256 }
1257
1258 int is_negative_m3(float mat[][3])
1259 {
1260         float vec[3];
1261         cross_v3_v3v3(vec, mat[0], mat[1]);
1262         return (dot_v3v3(vec, mat[2]) < 0.0f);
1263 }
1264
1265 int is_negative_m4(float mat[][4])
1266 {
1267         float vec[3];
1268         cross_v3_v3v3(vec, mat[0], mat[1]);
1269         return (dot_v3v3(vec, mat[2]) < 0.0f);
1270 }
1271
1272 /* make a 4x4 matrix out of 3 transform components */
1273 /* matrices are made in the order: scale * rot * loc */
1274 /* TODO: need to have a version that allows for rotation order... */
1275
1276 void loc_eul_size_to_mat4(float mat[4][4], const float loc[3], const float eul[3], const float size[3])
1277 {
1278         float rmat[3][3], smat[3][3], tmat[3][3];
1279
1280         /* initialize new matrix */
1281         unit_m4(mat);
1282
1283         /* make rotation + scaling part */
1284         eul_to_mat3(rmat, eul);
1285         size_to_mat3(smat, size);
1286         mul_m3_m3m3(tmat, rmat, smat);
1287
1288         /* copy rot/scale part to output matrix*/
1289         copy_m4_m3(mat, tmat);
1290
1291         /* copy location to matrix */
1292         mat[3][0] = loc[0];
1293         mat[3][1] = loc[1];
1294         mat[3][2] = loc[2];
1295 }
1296
1297 /* make a 4x4 matrix out of 3 transform components */
1298
1299 /* matrices are made in the order: scale * rot * loc */
1300 void loc_eulO_size_to_mat4(float mat[4][4], const float loc[3], const float eul[3], const float size[3], const short rotOrder)
1301 {
1302         float rmat[3][3], smat[3][3], tmat[3][3];
1303
1304         /* initialize new matrix */
1305         unit_m4(mat);
1306
1307         /* make rotation + scaling part */
1308         eulO_to_mat3(rmat, eul, rotOrder);
1309         size_to_mat3(smat, size);
1310         mul_m3_m3m3(tmat, rmat, smat);
1311
1312         /* copy rot/scale part to output matrix*/
1313         copy_m4_m3(mat, tmat);
1314
1315         /* copy location to matrix */
1316         mat[3][0] = loc[0];
1317         mat[3][1] = loc[1];
1318         mat[3][2] = loc[2];
1319 }
1320
1321
1322 /* make a 4x4 matrix out of 3 transform components */
1323
1324 /* matrices are made in the order: scale * rot * loc */
1325 void loc_quat_size_to_mat4(float mat[4][4], const float loc[3], const float quat[4], const float size[3])
1326 {
1327         float rmat[3][3], smat[3][3], tmat[3][3];
1328
1329         /* initialize new matrix */
1330         unit_m4(mat);
1331
1332         /* make rotation + scaling part */
1333         quat_to_mat3(rmat, quat);
1334         size_to_mat3(smat, size);
1335         mul_m3_m3m3(tmat, rmat, smat);
1336
1337         /* copy rot/scale part to output matrix*/
1338         copy_m4_m3(mat, tmat);
1339
1340         /* copy location to matrix */
1341         mat[3][0] = loc[0];
1342         mat[3][1] = loc[1];
1343         mat[3][2] = loc[2];
1344 }
1345
1346 void loc_axisangle_size_to_mat4(float mat[4][4], const float loc[3], const float axis[3], const float angle, const float size[3])
1347 {
1348         float q[4];
1349         axis_angle_to_quat(q, axis, angle);
1350         loc_quat_size_to_mat4(mat, loc, q, size);
1351 }
1352
1353 /*********************************** Other ***********************************/
1354
1355 void print_m3(const char *str, float m[][3])
1356 {
1357         printf("%s\n", str);
1358         printf("%f %f %f\n", m[0][0], m[1][0], m[2][0]);
1359         printf("%f %f %f\n", m[0][1], m[1][1], m[2][1]);
1360         printf("%f %f %f\n", m[0][2], m[1][2], m[2][2]);
1361         printf("\n");
1362 }
1363
1364 void print_m4(const char *str, float m[][4])
1365 {
1366         printf("%s\n", str);
1367         printf("%f %f %f %f\n", m[0][0], m[1][0], m[2][0], m[3][0]);
1368         printf("%f %f %f %f\n", m[0][1], m[1][1], m[2][1], m[3][1]);
1369         printf("%f %f %f %f\n", m[0][2], m[1][2], m[2][2], m[3][2]);
1370         printf("%f %f %f %f\n", m[0][3], m[1][3], m[2][3], m[3][3]);
1371         printf("\n");
1372 }
1373
1374 /*********************************** SVD ************************************
1375  * from TNT matrix library
1376  *
1377  * Compute the Single Value Decomposition of an arbitrary matrix A
1378  * That is compute the 3 matrices U,W,V with U column orthogonal (m,n)
1379  * ,W a diagonal matrix and V an orthogonal square matrix s.t.
1380  * A = U.W.Vt. From this decomposition it is trivial to compute the
1381  * (pseudo-inverse) of A as Ainv = V.Winv.tranpose(U).
1382  */
1383
1384 void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4])
1385 {
1386         float A[4][4];
1387         float work1[4], work2[4];
1388         int m = 4;
1389         int n = 4;
1390         int maxiter = 200;
1391         int nu = minf(m, n);
1392
1393         float *work = work1;
1394         float *e = work2;
1395         float eps;
1396
1397         int i = 0, j = 0, k = 0, p, pp, iter;
1398
1399         // Reduce A to bidiagonal form, storing the diagonal elements
1400         // in s and the super-diagonal elements in e.
1401
1402         int nct = minf(m - 1, n);
1403         int nrt = maxf(0, minf(n - 2, m));
1404
1405         copy_m4_m4(A, A_);
1406         zero_m4(U);
1407         zero_v4(s);
1408
1409         for (k = 0; k < maxf(nct, nrt); k++) {
1410                 if (k < nct) {
1411
1412                         // Compute the transformation for the k-th column and
1413                         // place the k-th diagonal in s[k].
1414                         // Compute 2-norm of k-th column without under/overflow.
1415                         s[k] = 0;
1416                         for (i = k; i < m; i++) {
1417                                 s[k] = hypotf(s[k], A[i][k]);
1418                         }
1419                         if (s[k] != 0.0f) {
1420                                 float invsk;
1421                                 if (A[k][k] < 0.0f) {
1422                                         s[k] = -s[k];
1423                                 }
1424                                 invsk = 1.0f / s[k];
1425                                 for (i = k; i < m; i++) {
1426                                         A[i][k] *= invsk;
1427                                 }
1428                                 A[k][k] += 1.0f;
1429                         }
1430                         s[k] = -s[k];
1431                 }
1432                 for (j = k + 1; j < n; j++) {
1433                         if ((k < nct) && (s[k] != 0.0f)) {
1434
1435                                 // Apply the transformation.
1436
1437                                 float t = 0;
1438                                 for (i = k; i < m; i++) {
1439                                         t += A[i][k] * A[i][j];
1440                                 }
1441                                 t = -t / A[k][k];
1442                                 for (i = k; i < m; i++) {
1443                                         A[i][j] += t * A[i][k];
1444                                 }
1445                         }
1446
1447                         // Place the k-th row of A into e for the
1448                         // subsequent calculation of the row transformation.
1449
1450                         e[j] = A[k][j];
1451                 }
1452                 if (k < nct) {
1453
1454                         // Place the transformation in U for subsequent back
1455                         // multiplication.
1456
1457                         for (i = k; i < m; i++)
1458                                 U[i][k] = A[i][k];
1459                 }
1460                 if (k < nrt) {
1461
1462                         // Compute the k-th row transformation and place the
1463                         // k-th super-diagonal in e[k].
1464                         // Compute 2-norm without under/overflow.
1465                         e[k] = 0;
1466                         for (i = k + 1; i < n; i++) {
1467                                 e[k] = hypotf(e[k], e[i]);
1468                         }
1469                         if (e[k] != 0.0f) {
1470                                 float invek;
1471                                 if (e[k + 1] < 0.0f) {
1472                                         e[k] = -e[k];
1473                                 }
1474                                 invek = 1.0f / e[k];
1475                                 for (i = k + 1; i < n; i++) {
1476                                         e[i] *= invek;
1477                                 }
1478                                 e[k + 1] += 1.0f;
1479                         }
1480                         e[k] = -e[k];
1481                         if ((k + 1 < m) & (e[k] != 0.0f)) {
1482                                 float invek1;
1483
1484                                 // Apply the transformation.
1485
1486                                 for (i = k + 1; i < m; i++) {
1487                                         work[i] = 0.0f;
1488                                 }
1489                                 for (j = k + 1; j < n; j++) {
1490                                         for (i = k + 1; i < m; i++) {
1491                                                 work[i] += e[j] * A[i][j];
1492                                         }
1493                                 }
1494                                 invek1 = 1.0f / e[k + 1];
1495                                 for (j = k + 1; j < n; j++) {
1496                                         float t = -e[j] * invek1;
1497                                         for (i = k + 1; i < m; i++) {
1498                                                 A[i][j] += t * work[i];
1499                                         }
1500                                 }
1501                         }
1502
1503                         // Place the transformation in V for subsequent
1504                         // back multiplication.
1505
1506                         for (i = k + 1; i < n; i++)
1507                                 V[i][k] = e[i];
1508                 }
1509         }
1510
1511         // Set up the final bidiagonal matrix or order p.
1512
1513         p = minf(n, m + 1);
1514         if (nct < n) {
1515                 s[nct] = A[nct][nct];
1516         }
1517         if (m < p) {
1518                 s[p - 1] = 0.0f;
1519         }
1520         if (nrt + 1 < p) {
1521                 e[nrt] = A[nrt][p - 1];
1522         }
1523         e[p - 1] = 0.0f;
1524
1525         // If required, generate U.
1526
1527         for (j = nct; j < nu; j++) {
1528                 for (i = 0; i < m; i++) {
1529                         U[i][j] = 0.0f;
1530                 }
1531                 U[j][j] = 1.0f;
1532         }
1533         for (k = nct - 1; k >= 0; k--) {
1534                 if (s[k] != 0.0f) {
1535                         for (j = k + 1; j < nu; j++) {
1536                                 float t = 0;
1537                                 for (i = k; i < m; i++) {
1538                                         t += U[i][k] * U[i][j];
1539                                 }
1540                                 t = -t / U[k][k];
1541                                 for (i = k; i < m; i++) {
1542                                         U[i][j] += t * U[i][k];
1543                                 }
1544                         }
1545                         for (i = k; i < m; i++) {
1546                                 U[i][k] = -U[i][k];
1547                         }
1548                         U[k][k] = 1.0f + U[k][k];
1549                         for (i = 0; i < k - 1; i++) {
1550                                 U[i][k] = 0.0f;
1551                         }
1552                 }
1553                 else {
1554                         for (i = 0; i < m; i++) {
1555                                 U[i][k] = 0.0f;
1556                         }
1557                         U[k][k] = 1.0f;
1558                 }
1559         }
1560
1561         // If required, generate V.
1562
1563         for (k = n - 1; k >= 0; k--) {
1564                 if ((k < nrt) & (e[k] != 0.0f)) {
1565                         for (j = k + 1; j < nu; j++) {
1566                                 float t = 0;
1567                                 for (i = k + 1; i < n; i++) {
1568                                         t += V[i][k] * V[i][j];
1569                                 }
1570                                 t = -t / V[k + 1][k];
1571                                 for (i = k + 1; i < n; i++) {
1572                                         V[i][j] += t * V[i][k];
1573                                 }
1574                         }
1575                 }
1576                 for (i = 0; i < n; i++) {
1577                         V[i][k] = 0.0f;
1578                 }
1579                 V[k][k] = 1.0f;
1580         }
1581
1582         // Main iteration loop for the singular values.
1583
1584         pp = p - 1;
1585         iter = 0;
1586         eps = powf(2.0f, -52.0f);
1587         while (p > 0) {
1588                 int kase = 0;
1589
1590                 // Test for maximum iterations to avoid infinite loop
1591                 if (maxiter == 0)
1592                         break;
1593                 maxiter--;
1594
1595                 // This section of the program inspects for
1596                 // negligible elements in the s and e arrays.  On
1597                 // completion the variables kase and k are set as follows.
1598
1599                 // kase = 1       if s(p) and e[k - 1] are negligible and k<p
1600                 // kase = 2       if s(k) is negligible and k<p
1601                 // kase = 3       if e[k - 1] is negligible, k<p, and
1602                 //               s(k), ..., s(p) are not negligible (qr step).
1603                 // kase = 4       if e(p - 1) is negligible (convergence).
1604
1605                 for (k = p - 2; k >= -1; k--) {
1606                         if (k == -1) {
1607                                 break;
1608                         }
1609                         if (fabsf(e[k]) <= eps * (fabsf(s[k]) + fabsf(s[k + 1]))) {
1610                                 e[k] = 0.0f;
1611                                 break;
1612                         }
1613                 }
1614                 if (k == p - 2) {
1615                         kase = 4;
1616                 }
1617                 else {
1618                         int ks;
1619                         for (ks = p - 1; ks >= k; ks--) {
1620                                 float t;
1621                                 if (ks == k) {
1622                                         break;
1623                                 }
1624                                 t = (ks != p ? fabsf(e[ks]) : 0.f) +
1625                                     (ks != k + 1 ? fabsf(e[ks - 1]) : 0.0f);
1626                                 if (fabsf(s[ks]) <= eps * t) {
1627                                         s[ks] = 0.0f;
1628                                         break;
1629                                 }
1630                         }
1631                         if (ks == k) {
1632                                 kase = 3;
1633                         }
1634                         else if (ks == p - 1) {
1635                                 kase = 1;
1636                         }
1637                         else {
1638                                 kase = 2;
1639                                 k = ks;
1640                         }
1641                 }
1642                 k++;
1643
1644                 // Perform the task indicated by kase.
1645
1646                 switch (kase) {
1647
1648                         // Deflate negligible s(p).
1649
1650                         case 1:
1651                         {
1652                                 float f = e[p - 2];
1653                                 e[p - 2] = 0.0f;
1654                                 for (j = p - 2; j >= k; j--) {
1655                                         float t = hypotf(s[j], f);
1656                                         float invt = 1.0f / t;
1657                                         float cs = s[j] * invt;
1658                                         float sn = f * invt;
1659                                         s[j] = t;
1660                                         if (j != k) {
1661                                                 f = -sn * e[j - 1];
1662                                                 e[j - 1] = cs * e[j - 1];
1663                                         }
1664
1665                                         for (i = 0; i < n; i++) {
1666                                                 t = cs * V[i][j] + sn * V[i][p - 1];
1667                                                 V[i][p - 1] = -sn * V[i][j] + cs * V[i][p - 1];
1668                                                 V[i][j] = t;
1669                                         }
1670                                 }
1671                                 break;
1672                         }
1673
1674                         // Split at negligible s(k).
1675
1676                         case 2:
1677                         {
1678                                 float f = e[k - 1];
1679                                 e[k - 1] = 0.0f;
1680                                 for (j = k; j < p; j++) {
1681                                         float t = hypotf(s[j], f);
1682                                         float invt = 1.0f / t;
1683                                         float cs = s[j] * invt;
1684                                         float sn = f * invt;
1685                                         s[j] = t;
1686                                         f = -sn * e[j];
1687                                         e[j] = cs * e[j];
1688
1689                                         for (i = 0; i < m; i++) {
1690                                                 t = cs * U[i][j] + sn * U[i][k - 1];
1691                                                 U[i][k - 1] = -sn * U[i][j] + cs * U[i][k - 1];
1692                                                 U[i][j] = t;
1693                                         }
1694                                 }
1695                                 break;
1696                         }
1697
1698                         // Perform one qr step.
1699
1700                         case 3:
1701                         {
1702
1703                                 // Calculate the shift.
1704
1705                                 float scale = maxf(maxf(maxf(maxf(
1706                                                    fabsf(s[p - 1]), fabsf(s[p - 2])), fabsf(e[p - 2])),
1707                                                    fabsf(s[k])), fabsf(e[k]));
1708                                 float invscale = 1.0f / scale;
1709                                 float sp = s[p - 1] * invscale;
1710                                 float spm1 = s[p - 2] * invscale;
1711                                 float epm1 = e[p - 2] * invscale;
1712                                 float sk = s[k] * invscale;
1713                                 float ek = e[k] * invscale;
1714                                 float b = ((spm1 + sp) * (spm1 - sp) + epm1 * epm1) * 0.5f;
1715                                 float c = (sp * epm1) * (sp * epm1);
1716                                 float shift = 0.0f;
1717                                 float f, g;
1718                                 if ((b != 0.0f) || (c != 0.0f)) {
1719                                         shift = sqrtf(b * b + c);
1720                                         if (b < 0.0f) {
1721                                                 shift = -shift;
1722                                         }
1723                                         shift = c / (b + shift);
1724                                 }
1725                                 f = (sk + sp) * (sk - sp) + shift;
1726                                 g = sk * ek;
1727
1728                                 // Chase zeros.
1729
1730                                 for (j = k; j < p - 1; j++) {
1731                                         float t = hypotf(f, g);
1732                                         /* division by zero checks added to avoid NaN (brecht) */
1733                                         float cs = (t == 0.0f) ? 0.0f : f / t;
1734                                         float sn = (t == 0.0f) ? 0.0f : g / t;
1735                                         if (j != k) {
1736                                                 e[j - 1] = t;
1737                                         }
1738                                         f = cs * s[j] + sn * e[j];
1739                                         e[j] = cs * e[j] - sn * s[j];
1740                                         g = sn * s[j + 1];
1741                                         s[j + 1] = cs * s[j + 1];
1742
1743                                         for (i = 0; i < n; i++) {
1744                                                 t = cs * V[i][j] + sn * V[i][j + 1];
1745                                                 V[i][j + 1] = -sn * V[i][j] + cs * V[i][j + 1];
1746                                                 V[i][j] = t;
1747                                         }
1748
1749                                         t = hypotf(f, g);
1750                                         /* division by zero checks added to avoid NaN (brecht) */
1751                                         cs = (t == 0.0f) ? 0.0f : f / t;
1752                                         sn = (t == 0.0f) ? 0.0f : g / t;
1753                                         s[j] = t;
1754                                         f = cs * e[j] + sn * s[j + 1];
1755                                         s[j + 1] = -sn * e[j] + cs * s[j + 1];
1756                                         g = sn * e[j + 1];
1757                                         e[j + 1] = cs * e[j + 1];
1758                                         if (j < m - 1) {
1759                                                 for (i = 0; i < m; i++) {
1760                                                         t = cs * U[i][j] + sn * U[i][j + 1];
1761                                                         U[i][j + 1] = -sn * U[i][j] + cs * U[i][j + 1];
1762                                                         U[i][j] = t;
1763                                                 }
1764                                         }
1765                                 }
1766                                 e[p - 2] = f;
1767                                 iter = iter + 1;
1768                                 break;
1769                         }
1770                         // Convergence.
1771
1772                         case 4:
1773                         {
1774
1775                                 // Make the singular values positive.
1776
1777                                 if (s[k] <= 0.0f) {
1778                                         s[k] = (s[k] < 0.0f ? -s[k] : 0.0f);
1779
1780                                         for (i = 0; i <= pp; i++)
1781                                                 V[i][k] = -V[i][k];
1782                                 }
1783
1784                                 // Order the singular values.
1785
1786                                 while (k < pp) {
1787                                         float t;
1788                                         if (s[k] >= s[k + 1]) {
1789                                                 break;
1790                                         }
1791                                         t = s[k];
1792                                         s[k] = s[k + 1];
1793                                         s[k + 1] = t;
1794                                         if (k < n - 1) {
1795                                                 for (i = 0; i < n; i++) {
1796                                                         t = V[i][k + 1];
1797                                                         V[i][k + 1] = V[i][k];
1798                                                         V[i][k] = t;
1799                                                 }
1800                                         }
1801                                         if (k < m - 1) {
1802                                                 for (i = 0; i < m; i++) {
1803                                                         t = U[i][k + 1];
1804                                                         U[i][k + 1] = U[i][k];
1805                                                         U[i][k] = t;
1806                                                 }
1807                                         }
1808                                         k++;
1809                                 }
1810                                 iter = 0;
1811                                 p--;
1812                                 break;
1813                         }
1814                 }
1815         }
1816 }
1817
1818 void pseudoinverse_m4_m4(float Ainv[4][4], float A[4][4], float epsilon)
1819 {
1820         /* compute moon-penrose pseudo inverse of matrix, singular values
1821          * below epsilon are ignored for stability (truncated SVD) */
1822         float V[4][4], W[4], Wm[4][4], U[4][4];
1823         int i;
1824
1825         transpose_m4(A);
1826         svd_m4(V, W, U, A);
1827         transpose_m4(U);
1828         transpose_m4(V);
1829
1830         zero_m4(Wm);
1831         for (i = 0; i < 4; i++)
1832                 Wm[i][i] = (W[i] < epsilon) ? 0.0f : 1.0f / W[i];
1833
1834         transpose_m4(V);
1835
1836         mul_serie_m4(Ainv, U, Wm, V, NULL, NULL, NULL, NULL, NULL);
1837 }