Merging r58475 through r58700 from trunk into soc-2013-depsgraph_mt
[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][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][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][3], float m2[3][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][4], float m2[4][4])
70 {
71         memcpy(m1, m2, 4 * 4 * sizeof(float));
72 }
73
74 void copy_m3_m4(float m1[3][3], float m2[4][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][4], float m2[3][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][3], float m2[3][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][4], float m2[4][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 mul_m4_m4m4(float m1[4][4], float m3_[4][4], float m2_[4][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][3], float m3_[3][3], float m2_[3][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][4], float m3_[4][4], float m2_[3][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 mul_m3_m3m4(float m1[3][3], float m3_[4][4], float m2_[3][3])
219 {
220         float m2[3][3], m3[4][4];
221
222         /* copy so it works when m1 is the same pointer as m2 or m3 */
223         copy_m3_m3(m2, m2_);
224         copy_m4_m4(m3, m3_);
225
226         /* m1[i][j] = m2[i][k] * m3[k][j] */
227         m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] + m2[0][2] * m3[2][0];
228         m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] + m2[0][2] * m3[2][1];
229         m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] + m2[0][2] * m3[2][2];
230
231         m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] + m2[1][2] * m3[2][0];
232         m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] + m2[1][2] * m3[2][1];
233         m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] + m2[1][2] * m3[2][2];
234
235         m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] + m2[2][2] * m3[2][0];
236         m1[2][1] = m2[2][0] * m3[0][1] + m2[2][1] * m3[1][1] + m2[2][2] * m3[2][1];
237         m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] + m2[2][2] * m3[2][2];
238 }
239
240 void mul_m4_m3m4(float m1[4][4], float m3_[3][3], float m2_[4][4])
241 {
242         float m2[4][4], m3[3][3];
243
244         /* copy so it works when m1 is the same pointer as m2 or m3 */
245         copy_m4_m4(m2, m2_);
246         copy_m3_m3(m3, m3_);
247
248         m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] + m2[0][2] * m3[2][0];
249         m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] + m2[0][2] * m3[2][1];
250         m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] + m2[0][2] * m3[2][2];
251         m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] + m2[1][2] * m3[2][0];
252         m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] + m2[1][2] * m3[2][1];
253         m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] + m2[1][2] * m3[2][2];
254         m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] + m2[2][2] * m3[2][0];
255         m1[2][1] = m2[2][0] * m3[0][1] + m2[2][1] * m3[1][1] + m2[2][2] * m3[2][1];
256         m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] + m2[2][2] * m3[2][2];
257 }
258
259 void mul_serie_m3(float answ[3][3],
260                   float m1[3][3], float m2[3][3], float m3[3][3],
261                   float m4[3][3], float m5[3][3], float m6[3][3],
262                   float m7[3][3], float m8[3][3])
263 {
264         float temp[3][3];
265
266         if (m1 == NULL || m2 == NULL) return;
267
268         mul_m3_m3m3(answ, m2, m1);
269         if (m3) {
270                 mul_m3_m3m3(temp, m3, answ);
271                 if (m4) {
272                         mul_m3_m3m3(answ, m4, temp);
273                         if (m5) {
274                                 mul_m3_m3m3(temp, m5, answ);
275                                 if (m6) {
276                                         mul_m3_m3m3(answ, m6, temp);
277                                         if (m7) {
278                                                 mul_m3_m3m3(temp, m7, answ);
279                                                 if (m8) {
280                                                         mul_m3_m3m3(answ, m8, temp);
281                                                 }
282                                                 else copy_m3_m3(answ, temp);
283                                         }
284                                 }
285                                 else copy_m3_m3(answ, temp);
286                         }
287                 }
288                 else copy_m3_m3(answ, temp);
289         }
290 }
291
292 void mul_serie_m4(float answ[4][4], float m1[4][4],
293                   float m2[4][4], float m3[4][4], float m4[4][4],
294                   float m5[4][4], float m6[4][4], float m7[4][4],
295                   float m8[4][4])
296 {
297         float temp[4][4];
298
299         if (m1 == NULL || m2 == NULL) return;
300
301         mul_m4_m4m4(answ, m1, m2);
302         if (m3) {
303                 mul_m4_m4m4(temp, answ, m3);
304                 if (m4) {
305                         mul_m4_m4m4(answ, temp, m4);
306                         if (m5) {
307                                 mul_m4_m4m4(temp, answ, m5);
308                                 if (m6) {
309                                         mul_m4_m4m4(answ, temp, m6);
310                                         if (m7) {
311                                                 mul_m4_m4m4(temp, answ, m7);
312                                                 if (m8) {
313                                                         mul_m4_m4m4(answ, temp, m8);
314                                                 }
315                                                 else copy_m4_m4(answ, temp);
316                                         }
317                                 }
318                                 else copy_m4_m4(answ, temp);
319                         }
320                 }
321                 else copy_m4_m4(answ, temp);
322         }
323 }
324
325 void mul_m4_v3(float mat[4][4], float vec[3])
326 {
327         float x, y;
328
329         x = vec[0];
330         y = vec[1];
331         vec[0] = x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2] + mat[3][0];
332         vec[1] = x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2] + mat[3][1];
333         vec[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2] + mat[3][2];
334 }
335
336 void mul_v3_m4v3(float r[3], float mat[4][4], const float vec[3])
337 {
338         float x, y;
339
340         x = vec[0];
341         y = vec[1];
342         r[0] = x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2] + mat[3][0];
343         r[1] = x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2] + mat[3][1];
344         r[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2] + mat[3][2];
345 }
346
347 void mul_v2_m4v3(float r[2], float mat[4][4], const float vec[3])
348 {
349         float x;
350
351         x = vec[0];
352         r[0] = x * mat[0][0] + vec[1] * mat[1][0] + mat[2][0] * vec[2] + mat[3][0];
353         r[1] = x * mat[0][1] + vec[1] * mat[1][1] + mat[2][1] * vec[2] + mat[3][1];
354 }
355
356 void mul_v2_m2v2(float r[2], float mat[2][2], const float vec[2])
357 {
358         float x;
359
360         x = vec[0];
361         r[0] = mat[0][0] * x + mat[1][0] * vec[1];
362         r[1] = mat[0][1] * x + mat[1][1] * vec[1];
363 }
364
365 /* same as mul_m4_v3() but doesnt apply translation component */
366 void mul_mat3_m4_v3(float mat[4][4], float vec[3])
367 {
368         float x, y;
369
370         x = vec[0];
371         y = vec[1];
372         vec[0] = x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2];
373         vec[1] = x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2];
374         vec[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2];
375 }
376
377 void mul_project_m4_v3(float mat[4][4], float vec[3])
378 {
379         const float w = mul_project_m4_v3_zfac(mat, vec);
380         mul_m4_v3(mat, vec);
381
382         vec[0] /= w;
383         vec[1] /= w;
384         vec[2] /= w;
385 }
386
387 void mul_v2_project_m4_v3(float r[2], float mat[4][4], const float vec[3])
388 {
389         const float w = mul_project_m4_v3_zfac(mat, vec);
390         mul_v2_m4v3(r, mat, vec);
391
392         r[0] /= w;
393         r[1] /= w;
394 }
395
396 void mul_v4_m4v4(float r[4], float mat[4][4], const float v[4])
397 {
398         float x, y, z;
399
400         x = v[0];
401         y = v[1];
402         z = v[2];
403
404         r[0] = x * mat[0][0] + y * mat[1][0] + z * mat[2][0] + mat[3][0] * v[3];
405         r[1] = x * mat[0][1] + y * mat[1][1] + z * mat[2][1] + mat[3][1] * v[3];
406         r[2] = x * mat[0][2] + y * mat[1][2] + z * mat[2][2] + mat[3][2] * v[3];
407         r[3] = x * mat[0][3] + y * mat[1][3] + z * mat[2][3] + mat[3][3] * v[3];
408 }
409
410 void mul_m4_v4(float mat[4][4], float r[4])
411 {
412         mul_v4_m4v4(r, mat, r);
413 }
414
415 void mul_v4d_m4v4d(double r[4], float mat[4][4], double v[4])
416 {
417         double x, y, z;
418
419         x = v[0];
420         y = v[1];
421         z = v[2];
422
423         r[0] = x * (double)mat[0][0] + y * (double)mat[1][0] + z * (double)mat[2][0] + (double)mat[3][0] * v[3];
424         r[1] = x * (double)mat[0][1] + y * (double)mat[1][1] + z * (double)mat[2][1] + (double)mat[3][1] * v[3];
425         r[2] = x * (double)mat[0][2] + y * (double)mat[1][2] + z * (double)mat[2][2] + (double)mat[3][2] * v[3];
426         r[3] = x * (double)mat[0][3] + y * (double)mat[1][3] + z * (double)mat[2][3] + (double)mat[3][3] * v[3];
427 }
428
429 void mul_m4_v4d(float mat[4][4], double r[4])
430 {
431         mul_v4d_m4v4d(r, mat, r);
432 }
433
434 void mul_v3_m3v3(float r[3], float M[3][3], const float a[3])
435 {
436         r[0] = M[0][0] * a[0] + M[1][0] * a[1] + M[2][0] * a[2];
437         r[1] = M[0][1] * a[0] + M[1][1] * a[1] + M[2][1] * a[2];
438         r[2] = M[0][2] * a[0] + M[1][2] * a[1] + M[2][2] * a[2];
439 }
440
441 void mul_v2_m3v3(float r[2], float M[3][3], const float a[3])
442 {
443         r[0] = M[0][0] * a[0] + M[1][0] * a[1] + M[2][0] * a[2];
444         r[1] = M[0][1] * a[0] + M[1][1] * a[1] + M[2][1] * a[2];
445 }
446
447 void mul_m3_v3(float M[3][3], float r[3])
448 {
449         float tmp[3];
450
451         mul_v3_m3v3(tmp, M, r);
452         copy_v3_v3(r, tmp);
453 }
454
455 void mul_transposed_m3_v3(float mat[3][3], float vec[3])
456 {
457         float x, y;
458
459         x = vec[0];
460         y = vec[1];
461         vec[0] = x * mat[0][0] + y * mat[0][1] + mat[0][2] * vec[2];
462         vec[1] = x * mat[1][0] + y * mat[1][1] + mat[1][2] * vec[2];
463         vec[2] = x * mat[2][0] + y * mat[2][1] + mat[2][2] * vec[2];
464 }
465
466 void mul_transposed_mat3_m4_v3(float mat[4][4], float vec[3])
467 {
468         float x, y;
469
470         x = vec[0];
471         y = vec[1];
472         vec[0] = x * mat[0][0] + y * mat[0][1] + mat[0][2] * vec[2];
473         vec[1] = x * mat[1][0] + y * mat[1][1] + mat[1][2] * vec[2];
474         vec[2] = x * mat[2][0] + y * mat[2][1] + mat[2][2] * vec[2];
475 }
476
477
478 void mul_m3_fl(float m[3][3], float f)
479 {
480         int i, j;
481
482         for (i = 0; i < 3; i++)
483                 for (j = 0; j < 3; j++)
484                         m[i][j] *= f;
485 }
486
487 void mul_m4_fl(float m[4][4], float f)
488 {
489         int i, j;
490
491         for (i = 0; i < 4; i++)
492                 for (j = 0; j < 4; j++)
493                         m[i][j] *= f;
494 }
495
496 void mul_mat3_m4_fl(float m[4][4], float f)
497 {
498         int i, j;
499
500         for (i = 0; i < 3; i++)
501                 for (j = 0; j < 3; j++)
502                         m[i][j] *= f;
503 }
504
505 void mul_m3_v3_double(float mat[3][3], double vec[3])
506 {
507         double x, y;
508
509         x = vec[0];
510         y = vec[1];
511         vec[0] = x * (double)mat[0][0] + y * (double)mat[1][0] + (double)mat[2][0] * vec[2];
512         vec[1] = x * (double)mat[0][1] + y * (double)mat[1][1] + (double)mat[2][1] * vec[2];
513         vec[2] = x * (double)mat[0][2] + y * (double)mat[1][2] + (double)mat[2][2] * vec[2];
514 }
515
516 void add_m3_m3m3(float m1[3][3], float m2[3][3], float m3[3][3])
517 {
518         int i, j;
519
520         for (i = 0; i < 3; i++)
521                 for (j = 0; j < 3; j++)
522                         m1[i][j] = m2[i][j] + m3[i][j];
523 }
524
525 void add_m4_m4m4(float m1[4][4], float m2[4][4], float m3[4][4])
526 {
527         int i, j;
528
529         for (i = 0; i < 4; i++)
530                 for (j = 0; j < 4; j++)
531                         m1[i][j] = m2[i][j] + m3[i][j];
532 }
533
534 void sub_m3_m3m3(float m1[3][3], float m2[3][3], float m3[3][3])
535 {
536         int i, j;
537
538         for (i = 0; i < 3; i++)
539                 for (j = 0; j < 3; j++)
540                         m1[i][j] = m2[i][j] - m3[i][j];
541 }
542
543 void sub_m4_m4m4(float m1[4][4], float m2[4][4], float m3[4][4])
544 {
545         int i, j;
546
547         for (i = 0; i < 4; i++)
548                 for (j = 0; j < 4; j++)
549                         m1[i][j] = m2[i][j] - m3[i][j];
550 }
551
552 float determinant_m3_array(float m[3][3])
553 {
554         return (m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1]) -
555                 m[1][0] * (m[0][1] * m[2][2] - m[0][2] * m[2][1]) +
556                 m[2][0] * (m[0][1] * m[1][2] - m[0][2] * m[1][1]));
557 }
558
559 int invert_m3_ex(float m[3][3], const float epsilon)
560 {
561         float tmp[3][3];
562         int success;
563
564         success = invert_m3_m3_ex(tmp, m, epsilon);
565         copy_m3_m3(m, tmp);
566
567         return success;
568 }
569
570 int invert_m3_m3_ex(float m1[3][3], float m2[3][3], const float epsilon)
571 {
572         float det;
573         int a, b, success;
574
575         BLI_assert(epsilon >= 0.0f);
576
577         /* calc adjoint */
578         adjoint_m3_m3(m1, m2);
579
580         /* then determinant old matrix! */
581         det = determinant_m3_array(m2);
582
583         success = (fabsf(det) > epsilon);
584
585         if (LIKELY(det != 0.0f)) {
586                 det = 1.0f / det;
587                 for (a = 0; a < 3; a++) {
588                         for (b = 0; b < 3; b++) {
589                                 m1[a][b] *= det;
590                         }
591                 }
592         }
593         return success;
594 }
595
596 int invert_m3(float m[3][3])
597 {
598         float tmp[3][3];
599         int success;
600
601         success = invert_m3_m3(tmp, m);
602         copy_m3_m3(m, tmp);
603
604         return success;
605 }
606
607 int invert_m3_m3(float m1[3][3], float m2[3][3])
608 {
609         float det;
610         int a, b, success;
611
612         /* calc adjoint */
613         adjoint_m3_m3(m1, m2);
614
615         /* then determinant old matrix! */
616         det = determinant_m3_array(m2);
617
618         success = (det != 0.0f);
619
620         if (LIKELY(det != 0.0f)) {
621                 det = 1.0f / det;
622                 for (a = 0; a < 3; a++) {
623                         for (b = 0; b < 3; b++) {
624                                 m1[a][b] *= det;
625                         }
626                 }
627         }
628
629         return success;
630 }
631
632 int invert_m4(float m[4][4])
633 {
634         float tmp[4][4];
635         int success;
636
637         success = invert_m4_m4(tmp, m);
638         copy_m4_m4(m, tmp);
639
640         return success;
641 }
642
643 /*
644  * invertmat -
645  *      computes the inverse of mat and puts it in inverse.  Returns
646  *  TRUE on success (i.e. can always find a pivot) and FALSE on failure.
647  *  Uses Gaussian Elimination with partial (maximal column) pivoting.
648  *
649  *                                      Mark Segal - 1992
650  */
651
652 int invert_m4_m4(float inverse[4][4], float mat[4][4])
653 {
654         int i, j, k;
655         double temp;
656         float tempmat[4][4];
657         float max;
658         int maxj;
659
660         BLI_assert(inverse != mat);
661
662         /* Set inverse to identity */
663         for (i = 0; i < 4; i++)
664                 for (j = 0; j < 4; j++)
665                         inverse[i][j] = 0;
666         for (i = 0; i < 4; i++)
667                 inverse[i][i] = 1;
668
669         /* Copy original matrix so we don't mess it up */
670         for (i = 0; i < 4; i++)
671                 for (j = 0; j < 4; j++)
672                         tempmat[i][j] = mat[i][j];
673
674         for (i = 0; i < 4; i++) {
675                 /* Look for row with max pivot */
676                 max = fabs(tempmat[i][i]);
677                 maxj = i;
678                 for (j = i + 1; j < 4; j++) {
679                         if (fabsf(tempmat[j][i]) > max) {
680                                 max = fabs(tempmat[j][i]);
681                                 maxj = j;
682                         }
683                 }
684                 /* Swap rows if necessary */
685                 if (maxj != i) {
686                         for (k = 0; k < 4; k++) {
687                                 SWAP(float, tempmat[i][k], tempmat[maxj][k]);
688                                 SWAP(float, inverse[i][k], inverse[maxj][k]);
689                         }
690                 }
691
692                 temp = tempmat[i][i];
693                 if (temp == 0)
694                         return 0;  /* No non-zero pivot */
695                 for (k = 0; k < 4; k++) {
696                         tempmat[i][k] = (float)((double)tempmat[i][k] / temp);
697                         inverse[i][k] = (float)((double)inverse[i][k] / temp);
698                 }
699                 for (j = 0; j < 4; j++) {
700                         if (j != i) {
701                                 temp = tempmat[j][i];
702                                 for (k = 0; k < 4; k++) {
703                                         tempmat[j][k] -= (float)((double)tempmat[i][k] * temp);
704                                         inverse[j][k] -= (float)((double)inverse[i][k] * temp);
705                                 }
706                         }
707                 }
708         }
709         return 1;
710 }
711
712 /****************************** Linear Algebra *******************************/
713
714 void transpose_m3(float mat[3][3])
715 {
716         float t;
717
718         t = mat[0][1];
719         mat[0][1] = mat[1][0];
720         mat[1][0] = t;
721         t = mat[0][2];
722         mat[0][2] = mat[2][0];
723         mat[2][0] = t;
724         t = mat[1][2];
725         mat[1][2] = mat[2][1];
726         mat[2][1] = t;
727 }
728
729 void transpose_m4(float mat[4][4])
730 {
731         float t;
732
733         t = mat[0][1];
734         mat[0][1] = mat[1][0];
735         mat[1][0] = t;
736         t = mat[0][2];
737         mat[0][2] = mat[2][0];
738         mat[2][0] = t;
739         t = mat[0][3];
740         mat[0][3] = mat[3][0];
741         mat[3][0] = t;
742
743         t = mat[1][2];
744         mat[1][2] = mat[2][1];
745         mat[2][1] = t;
746         t = mat[1][3];
747         mat[1][3] = mat[3][1];
748         mat[3][1] = t;
749
750         t = mat[2][3];
751         mat[2][3] = mat[3][2];
752         mat[3][2] = t;
753 }
754
755 int compare_m4m4(float mat1[4][4], float mat2[4][4], float limit)
756 {
757         if (compare_v4v4(mat1[0], mat2[0], limit))
758                 if (compare_v4v4(mat1[1], mat2[1], limit))
759                         if (compare_v4v4(mat1[2], mat2[2], limit))
760                                 if (compare_v4v4(mat1[3], mat2[3], limit))
761                                         return 1;
762         return 0;
763 }
764
765 void orthogonalize_m3(float mat[3][3], int axis)
766 {
767         float size[3];
768         mat3_to_size(size, mat);
769         normalize_v3(mat[axis]);
770         switch (axis) {
771                 case 0:
772                         if (dot_v3v3(mat[0], mat[1]) < 1) {
773                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
774                                 normalize_v3(mat[2]);
775                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
776                         }
777                         else if (dot_v3v3(mat[0], mat[2]) < 1) {
778                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
779                                 normalize_v3(mat[1]);
780                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
781                         }
782                         else {
783                                 float vec[3];
784
785                                 vec[0] = mat[0][1];
786                                 vec[1] = mat[0][2];
787                                 vec[2] = mat[0][0];
788
789                                 cross_v3_v3v3(mat[2], mat[0], vec);
790                                 normalize_v3(mat[2]);
791                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
792                         }
793                         break;
794                 case 1:
795                         if (dot_v3v3(mat[1], mat[0]) < 1) {
796                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
797                                 normalize_v3(mat[2]);
798                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
799                         }
800                         else if (dot_v3v3(mat[0], mat[2]) < 1) {
801                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
802                                 normalize_v3(mat[0]);
803                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
804                         }
805                         else {
806                                 float vec[3];
807
808                                 vec[0] = mat[1][1];
809                                 vec[1] = mat[1][2];
810                                 vec[2] = mat[1][0];
811
812                                 cross_v3_v3v3(mat[0], mat[1], vec);
813                                 normalize_v3(mat[0]);
814                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
815                         }
816                         break;
817                 case 2:
818                         if (dot_v3v3(mat[2], mat[0]) < 1) {
819                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
820                                 normalize_v3(mat[1]);
821                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
822                         }
823                         else if (dot_v3v3(mat[2], mat[1]) < 1) {
824                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
825                                 normalize_v3(mat[0]);
826                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
827                         }
828                         else {
829                                 float vec[3];
830
831                                 vec[0] = mat[2][1];
832                                 vec[1] = mat[2][2];
833                                 vec[2] = mat[2][0];
834
835                                 cross_v3_v3v3(mat[0], vec, mat[2]);
836                                 normalize_v3(mat[0]);
837                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
838                         }
839                         break;
840                 default:
841                         BLI_assert(0);
842                         break;
843         }
844         mul_v3_fl(mat[0], size[0]);
845         mul_v3_fl(mat[1], size[1]);
846         mul_v3_fl(mat[2], size[2]);
847 }
848
849 void orthogonalize_m4(float mat[4][4], int axis)
850 {
851         float size[3];
852         mat4_to_size(size, mat);
853         normalize_v3(mat[axis]);
854         switch (axis) {
855                 case 0:
856                         if (dot_v3v3(mat[0], mat[1]) < 1) {
857                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
858                                 normalize_v3(mat[2]);
859                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
860                         }
861                         else if (dot_v3v3(mat[0], mat[2]) < 1) {
862                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
863                                 normalize_v3(mat[1]);
864                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
865                         }
866                         else {
867                                 float vec[3];
868
869                                 vec[0] = mat[0][1];
870                                 vec[1] = mat[0][2];
871                                 vec[2] = mat[0][0];
872
873                                 cross_v3_v3v3(mat[2], mat[0], vec);
874                                 normalize_v3(mat[2]);
875                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
876                         }
877                         break;
878                 case 1:
879                         if (dot_v3v3(mat[1], mat[0]) < 1) {
880                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
881                                 normalize_v3(mat[2]);
882                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
883                         }
884                         else if (dot_v3v3(mat[0], mat[2]) < 1) {
885                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
886                                 normalize_v3(mat[0]);
887                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
888                         }
889                         else {
890                                 float vec[3];
891
892                                 vec[0] = mat[1][1];
893                                 vec[1] = mat[1][2];
894                                 vec[2] = mat[1][0];
895
896                                 cross_v3_v3v3(mat[0], mat[1], vec);
897                                 normalize_v3(mat[0]);
898                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
899                         }
900                         break;
901                 case 2:
902                         if (dot_v3v3(mat[2], mat[0]) < 1) {
903                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
904                                 normalize_v3(mat[1]);
905                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
906                         }
907                         else if (dot_v3v3(mat[2], mat[1]) < 1) {
908                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
909                                 normalize_v3(mat[0]);
910                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
911                         }
912                         else {
913                                 float vec[3];
914
915                                 vec[0] = mat[2][1];
916                                 vec[1] = mat[2][2];
917                                 vec[2] = mat[2][0];
918
919                                 cross_v3_v3v3(mat[0], vec, mat[2]);
920                                 normalize_v3(mat[0]);
921                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
922                         }
923                         break;
924                 default:
925                         BLI_assert(0);
926                         break;
927         }
928         mul_v3_fl(mat[0], size[0]);
929         mul_v3_fl(mat[1], size[1]);
930         mul_v3_fl(mat[2], size[2]);
931 }
932
933 bool is_orthogonal_m3(float m[3][3])
934 {
935         int i, j;
936
937         for (i = 0; i < 3; i++) {
938                 for (j = 0; j < i; j++) {
939                         if (fabsf(dot_v3v3(m[i], m[j])) > 1.5f * FLT_EPSILON)
940                                 return 0;
941                 }
942         }
943
944         return 1;
945 }
946
947 bool is_orthogonal_m4(float m[4][4])
948 {
949         int i, j;
950
951         for (i = 0; i < 4; i++) {
952                 for (j = 0; j < i; j++) {
953                         if (fabsf(dot_vn_vn(m[i], m[j], 4)) > 1.5f * FLT_EPSILON)
954                                 return 0;
955                 }
956
957         }
958
959         return 1;
960 }
961
962 bool is_orthonormal_m3(float m[3][3])
963 {
964         if (is_orthogonal_m3(m)) {
965                 int i;
966
967                 for (i = 0; i < 3; i++)
968                         if (fabsf(dot_v3v3(m[i], m[i]) - 1) > 1.5f * FLT_EPSILON)
969                                 return 0;
970
971                 return 1;
972         }
973
974         return 0;
975 }
976
977 bool is_orthonormal_m4(float m[4][4])
978 {
979         if (is_orthogonal_m4(m)) {
980                 int i;
981
982                 for (i = 0; i < 4; i++)
983                         if (fabsf(dot_vn_vn(m[i], m[i], 4) - 1) > 1.5f * FLT_EPSILON)
984                                 return 0;
985
986                 return 1;
987         }
988
989         return 0;
990 }
991
992 bool is_uniform_scaled_m3(float m[3][3])
993 {
994         const float eps = 1e-7;
995         float t[3][3];
996         float l1, l2, l3, l4, l5, l6;
997
998         copy_m3_m3(t, m);
999         transpose_m3(t);
1000
1001         l1 = len_squared_v3(m[0]);
1002         l2 = len_squared_v3(m[1]);
1003         l3 = len_squared_v3(m[2]);
1004
1005         l4 = len_squared_v3(t[0]);
1006         l5 = len_squared_v3(t[1]);
1007         l6 = len_squared_v3(t[2]);
1008
1009         if (fabsf(l2 - l1) <= eps &&
1010             fabsf(l3 - l1) <= eps &&
1011             fabsf(l4 - l1) <= eps &&
1012             fabsf(l5 - l1) <= eps &&
1013             fabsf(l6 - l1) <= eps)
1014         {
1015                 return 1;
1016         }
1017
1018         return 0;
1019 }
1020
1021 void normalize_m3(float mat[3][3])
1022 {
1023         normalize_v3(mat[0]);
1024         normalize_v3(mat[1]);
1025         normalize_v3(mat[2]);
1026 }
1027
1028 void normalize_m3_m3(float rmat[3][3], float mat[3][3])
1029 {
1030         normalize_v3_v3(rmat[0], mat[0]);
1031         normalize_v3_v3(rmat[1], mat[1]);
1032         normalize_v3_v3(rmat[2], mat[2]);
1033 }
1034
1035 void normalize_m4(float mat[4][4])
1036 {
1037         float len;
1038
1039         len = normalize_v3(mat[0]);
1040         if (len != 0.0f) mat[0][3] /= len;
1041         len = normalize_v3(mat[1]);
1042         if (len != 0.0f) mat[1][3] /= len;
1043         len = normalize_v3(mat[2]);
1044         if (len != 0.0f) mat[2][3] /= len;
1045 }
1046
1047 void normalize_m4_m4(float rmat[4][4], float mat[4][4])
1048 {
1049         copy_m4_m4(rmat, mat);
1050         normalize_m4(rmat);
1051 }
1052
1053 void adjoint_m2_m2(float m1[2][2], float m[2][2])
1054 {
1055         BLI_assert(m1 != m);
1056         m1[0][0] =  m[1][1];
1057         m1[0][1] = -m[0][1];
1058         m1[1][0] = -m[1][0];
1059         m1[1][1] =  m[0][0];
1060 }
1061
1062 void adjoint_m3_m3(float m1[3][3], float m[3][3])
1063 {
1064         BLI_assert(m1 != m);
1065         m1[0][0] = m[1][1] * m[2][2] - m[1][2] * m[2][1];
1066         m1[0][1] = -m[0][1] * m[2][2] + m[0][2] * m[2][1];
1067         m1[0][2] = m[0][1] * m[1][2] - m[0][2] * m[1][1];
1068
1069         m1[1][0] = -m[1][0] * m[2][2] + m[1][2] * m[2][0];
1070         m1[1][1] = m[0][0] * m[2][2] - m[0][2] * m[2][0];
1071         m1[1][2] = -m[0][0] * m[1][2] + m[0][2] * m[1][0];
1072
1073         m1[2][0] = m[1][0] * m[2][1] - m[1][1] * m[2][0];
1074         m1[2][1] = -m[0][0] * m[2][1] + m[0][1] * m[2][0];
1075         m1[2][2] = m[0][0] * m[1][1] - m[0][1] * m[1][0];
1076 }
1077
1078 void adjoint_m4_m4(float out[4][4], float in[4][4]) /* out = ADJ(in) */
1079 {
1080         float a1, a2, a3, a4, b1, b2, b3, b4;
1081         float c1, c2, c3, c4, d1, d2, d3, d4;
1082
1083         a1 = in[0][0];
1084         b1 = in[0][1];
1085         c1 = in[0][2];
1086         d1 = in[0][3];
1087
1088         a2 = in[1][0];
1089         b2 = in[1][1];
1090         c2 = in[1][2];
1091         d2 = in[1][3];
1092
1093         a3 = in[2][0];
1094         b3 = in[2][1];
1095         c3 = in[2][2];
1096         d3 = in[2][3];
1097
1098         a4 = in[3][0];
1099         b4 = in[3][1];
1100         c4 = in[3][2];
1101         d4 = in[3][3];
1102
1103
1104         out[0][0] = determinant_m3(b2, b3, b4, c2, c3, c4, d2, d3, d4);
1105         out[1][0] = -determinant_m3(a2, a3, a4, c2, c3, c4, d2, d3, d4);
1106         out[2][0] = determinant_m3(a2, a3, a4, b2, b3, b4, d2, d3, d4);
1107         out[3][0] = -determinant_m3(a2, a3, a4, b2, b3, b4, c2, c3, c4);
1108
1109         out[0][1] = -determinant_m3(b1, b3, b4, c1, c3, c4, d1, d3, d4);
1110         out[1][1] = determinant_m3(a1, a3, a4, c1, c3, c4, d1, d3, d4);
1111         out[2][1] = -determinant_m3(a1, a3, a4, b1, b3, b4, d1, d3, d4);
1112         out[3][1] = determinant_m3(a1, a3, a4, b1, b3, b4, c1, c3, c4);
1113
1114         out[0][2] = determinant_m3(b1, b2, b4, c1, c2, c4, d1, d2, d4);
1115         out[1][2] = -determinant_m3(a1, a2, a4, c1, c2, c4, d1, d2, d4);
1116         out[2][2] = determinant_m3(a1, a2, a4, b1, b2, b4, d1, d2, d4);
1117         out[3][2] = -determinant_m3(a1, a2, a4, b1, b2, b4, c1, c2, c4);
1118
1119         out[0][3] = -determinant_m3(b1, b2, b3, c1, c2, c3, d1, d2, d3);
1120         out[1][3] = determinant_m3(a1, a2, a3, c1, c2, c3, d1, d2, d3);
1121         out[2][3] = -determinant_m3(a1, a2, a3, b1, b2, b3, d1, d2, d3);
1122         out[3][3] = determinant_m3(a1, a2, a3, b1, b2, b3, c1, c2, c3);
1123 }
1124
1125 float determinant_m2(float a, float b, float c, float d)
1126 {
1127
1128         return a * d - b * c;
1129 }
1130
1131 float determinant_m3(float a1, float a2, float a3,
1132                      float b1, float b2, float b3,
1133                      float c1, float c2, float c3)
1134 {
1135         float ans;
1136
1137         ans = (a1 * determinant_m2(b2, b3, c2, c3) -
1138                b1 * determinant_m2(a2, a3, c2, c3) +
1139                c1 * determinant_m2(a2, a3, b2, b3));
1140
1141         return ans;
1142 }
1143
1144 float determinant_m4(float m[4][4])
1145 {
1146         float ans;
1147         float a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4;
1148
1149         a1 = m[0][0];
1150         b1 = m[0][1];
1151         c1 = m[0][2];
1152         d1 = m[0][3];
1153
1154         a2 = m[1][0];
1155         b2 = m[1][1];
1156         c2 = m[1][2];
1157         d2 = m[1][3];
1158
1159         a3 = m[2][0];
1160         b3 = m[2][1];
1161         c3 = m[2][2];
1162         d3 = m[2][3];
1163
1164         a4 = m[3][0];
1165         b4 = m[3][1];
1166         c4 = m[3][2];
1167         d4 = m[3][3];
1168
1169         ans = (a1 * determinant_m3(b2, b3, b4, c2, c3, c4, d2, d3, d4) -
1170                b1 * determinant_m3(a2, a3, a4, c2, c3, c4, d2, d3, d4) +
1171                c1 * determinant_m3(a2, a3, a4, b2, b3, b4, d2, d3, d4) -
1172                d1 * determinant_m3(a2, a3, a4, b2, b3, b4, c2, c3, c4));
1173
1174         return ans;
1175 }
1176
1177 /****************************** Transformations ******************************/
1178
1179 void size_to_mat3(float mat[3][3], const float size[3])
1180 {
1181         mat[0][0] = size[0];
1182         mat[0][1] = 0.0f;
1183         mat[0][2] = 0.0f;
1184         mat[1][1] = size[1];
1185         mat[1][0] = 0.0f;
1186         mat[1][2] = 0.0f;
1187         mat[2][2] = size[2];
1188         mat[2][1] = 0.0f;
1189         mat[2][0] = 0.0f;
1190 }
1191
1192 void size_to_mat4(float mat[4][4], const float size[3])
1193 {
1194         float tmat[3][3];
1195
1196         size_to_mat3(tmat, size);
1197         unit_m4(mat);
1198         copy_m4_m3(mat, tmat);
1199 }
1200
1201 void mat3_to_size(float size[3], float mat[3][3])
1202 {
1203         size[0] = len_v3(mat[0]);
1204         size[1] = len_v3(mat[1]);
1205         size[2] = len_v3(mat[2]);
1206 }
1207
1208 void mat4_to_size(float size[3], float mat[4][4])
1209 {
1210         size[0] = len_v3(mat[0]);
1211         size[1] = len_v3(mat[1]);
1212         size[2] = len_v3(mat[2]);
1213 }
1214
1215 /* this gets the average scale of a matrix, only use when your scaling
1216  * data that has no idea of scale axis, examples are bone-envelope-radius
1217  * and curve radius */
1218 float mat3_to_scale(float mat[3][3])
1219 {
1220         /* unit length vector */
1221         float unit_vec[3];
1222         copy_v3_fl(unit_vec, 0.577350269189626f);
1223         mul_m3_v3(mat, unit_vec);
1224         return len_v3(unit_vec);
1225 }
1226
1227 float mat4_to_scale(float mat[4][4])
1228 {
1229         /* unit length vector */
1230         float unit_vec[3];
1231         copy_v3_fl(unit_vec, 0.577350269189626f);
1232         mul_mat3_m4_v3(mat, unit_vec);
1233         return len_v3(unit_vec);
1234 }
1235
1236 void mat3_to_rot_size(float rot[3][3], float size[3], float mat3[3][3])
1237 {
1238         float mat3_n[3][3]; /* mat3 -> normalized, 3x3 */
1239         float imat3_n[3][3]; /* mat3 -> normalized & inverted, 3x3 */
1240
1241         /* rotation & scale are linked, we need to create the mat's
1242          * for these together since they are related. */
1243
1244         /* so scale doesn't interfere with rotation [#24291] */
1245         /* note: this is a workaround for negative matrix not working for rotation conversion, FIXME */
1246         normalize_m3_m3(mat3_n, mat3);
1247         if (is_negative_m3(mat3)) {
1248                 negate_v3(mat3_n[0]);
1249                 negate_v3(mat3_n[1]);
1250                 negate_v3(mat3_n[2]);
1251         }
1252
1253         /* rotation */
1254         /* keep rot as a 3x3 matrix, the caller can convert into a quat or euler */
1255         copy_m3_m3(rot, mat3_n);
1256
1257         /* scale */
1258         /* note: mat4_to_size(ob->size, mat) fails for negative scale */
1259         invert_m3_m3(imat3_n, mat3_n);
1260         mul_m3_m3m3(mat3, imat3_n, mat3);
1261
1262         size[0] = mat3[0][0];
1263         size[1] = mat3[1][1];
1264         size[2] = mat3[2][2];
1265 }
1266
1267 void mat4_to_loc_rot_size(float loc[3], float rot[3][3], float size[3], float wmat[4][4])
1268 {
1269         float mat3[3][3]; /* wmat -> 3x3 */
1270
1271         copy_m3_m4(mat3, wmat);
1272         mat3_to_rot_size(rot, size, mat3);
1273
1274         /* location */
1275         copy_v3_v3(loc, wmat[3]);
1276 }
1277
1278 void mat4_to_loc_quat(float loc[3], float quat[4], float wmat[4][4])
1279 {
1280         float mat3[3][3];
1281         float mat3_n[3][3]; /* normalized mat3 */
1282
1283         copy_m3_m4(mat3, wmat);
1284         normalize_m3_m3(mat3_n, mat3);
1285
1286         /* so scale doesn't interfere with rotation [#24291] */
1287         /* note: this is a workaround for negative matrix not working for rotation conversion, FIXME */
1288         if (is_negative_m3(mat3)) {
1289                 negate_v3(mat3_n[0]);
1290                 negate_v3(mat3_n[1]);
1291                 negate_v3(mat3_n[2]);
1292         }
1293
1294         mat3_to_quat(quat, mat3_n);
1295         copy_v3_v3(loc, wmat[3]);
1296 }
1297
1298 void mat4_decompose(float loc[3], float quat[4], float size[3], float wmat[4][4])
1299 {
1300         float rot[3][3];
1301         mat4_to_loc_rot_size(loc, rot, size, wmat);
1302         mat3_to_quat(quat, rot);
1303 }
1304
1305 void scale_m3_fl(float m[3][3], float scale)
1306 {
1307         m[0][0] = m[1][1] = m[2][2] = scale;
1308         m[0][1] = m[0][2] = 0.0;
1309         m[1][0] = m[1][2] = 0.0;
1310         m[2][0] = m[2][1] = 0.0;
1311 }
1312
1313 void scale_m4_fl(float m[4][4], float scale)
1314 {
1315         m[0][0] = m[1][1] = m[2][2] = scale;
1316         m[3][3] = 1.0;
1317         m[0][1] = m[0][2] = m[0][3] = 0.0;
1318         m[1][0] = m[1][2] = m[1][3] = 0.0;
1319         m[2][0] = m[2][1] = m[2][3] = 0.0;
1320         m[3][0] = m[3][1] = m[3][2] = 0.0;
1321 }
1322
1323 void translate_m4(float mat[4][4], float Tx, float Ty, float Tz)
1324 {
1325         mat[3][0] += (Tx * mat[0][0] + Ty * mat[1][0] + Tz * mat[2][0]);
1326         mat[3][1] += (Tx * mat[0][1] + Ty * mat[1][1] + Tz * mat[2][1]);
1327         mat[3][2] += (Tx * mat[0][2] + Ty * mat[1][2] + Tz * mat[2][2]);
1328 }
1329
1330 void rotate_m4(float mat[4][4], const char axis, const float angle)
1331 {
1332         int col;
1333         float temp[4] = {0.0f, 0.0f, 0.0f, 0.0f};
1334         float cosine, sine;
1335
1336         assert(axis >= 'X' && axis <= 'Z');
1337
1338         cosine = cosf(angle);
1339         sine   = sinf(angle);
1340         switch (axis) {
1341                 case 'X':
1342                         for (col = 0; col < 4; col++)
1343                                 temp[col] = cosine * mat[1][col] + sine * mat[2][col];
1344                         for (col = 0; col < 4; col++) {
1345                                 mat[2][col] = -sine * mat[1][col] + cosine * mat[2][col];
1346                                 mat[1][col] = temp[col];
1347                         }
1348                         break;
1349
1350                 case 'Y':
1351                         for (col = 0; col < 4; col++)
1352                                 temp[col] = cosine * mat[0][col] - sine * mat[2][col];
1353                         for (col = 0; col < 4; col++) {
1354                                 mat[2][col] = sine * mat[0][col] + cosine * mat[2][col];
1355                                 mat[0][col] = temp[col];
1356                         }
1357                         break;
1358
1359                 case 'Z':
1360                         for (col = 0; col < 4; col++)
1361                                 temp[col] = cosine * mat[0][col] + sine * mat[1][col];
1362                         for (col = 0; col < 4; col++) {
1363                                 mat[1][col] = -sine * mat[0][col] + cosine * mat[1][col];
1364                                 mat[0][col] = temp[col];
1365                         }
1366                         break;
1367         }
1368 }
1369
1370 void rotate_m2(float mat[2][2], const float angle)
1371 {
1372         mat[0][0] = mat[1][1] = cosf(angle);
1373         mat[0][1] = sinf(angle);
1374         mat[1][0] = -mat[0][1];
1375 }
1376
1377 /* scale or rotate around a non zero pivot */
1378 void pivot_m4(float mat[4][4], const float pivot[3])
1379 {
1380         float tmat[4][4];
1381
1382         unit_m4(tmat);
1383
1384         copy_v3_v3(tmat[3], pivot);
1385         mul_m4_m4m4(mat, tmat, mat);
1386
1387         /* invert the matrix */
1388         negate_v3(tmat[3]);
1389         mul_m4_m4m4(mat, mat, tmat);
1390 }
1391
1392 void blend_m3_m3m3(float out[3][3], float dst[3][3], float src[3][3], const float srcweight)
1393 {
1394         float srot[3][3], drot[3][3];
1395         float squat[4], dquat[4], fquat[4];
1396         float sscale[3], dscale[3], fsize[3];
1397         float rmat[3][3], smat[3][3];
1398
1399         mat3_to_rot_size(drot, dscale, dst);
1400         mat3_to_rot_size(srot, sscale, src);
1401
1402         mat3_to_quat(dquat, drot);
1403         mat3_to_quat(squat, srot);
1404
1405         /* do blending */
1406         interp_qt_qtqt(fquat, dquat, squat, srcweight);
1407         interp_v3_v3v3(fsize, dscale, sscale, srcweight);
1408
1409         /* compose new matrix */
1410         quat_to_mat3(rmat, fquat);
1411         size_to_mat3(smat, fsize);
1412         mul_m3_m3m3(out, rmat, smat);
1413 }
1414
1415 void blend_m4_m4m4(float out[4][4], float dst[4][4], float src[4][4], const float srcweight)
1416 {
1417         float sloc[3], dloc[3], floc[3];
1418         float srot[3][3], drot[3][3];
1419         float squat[4], dquat[4], fquat[4];
1420         float sscale[3], dscale[3], fsize[3];
1421
1422         mat4_to_loc_rot_size(dloc, drot, dscale, dst);
1423         mat4_to_loc_rot_size(sloc, srot, sscale, src);
1424
1425         mat3_to_quat(dquat, drot);
1426         mat3_to_quat(squat, srot);
1427
1428         /* do blending */
1429         interp_v3_v3v3(floc, dloc, sloc, srcweight);
1430         interp_qt_qtqt(fquat, dquat, squat, srcweight);
1431         interp_v3_v3v3(fsize, dscale, sscale, srcweight);
1432
1433         /* compose new matrix */
1434         loc_quat_size_to_mat4(out, floc, fquat, fsize);
1435 }
1436
1437 bool is_negative_m3(float mat[3][3])
1438 {
1439         float vec[3];
1440         cross_v3_v3v3(vec, mat[0], mat[1]);
1441         return (dot_v3v3(vec, mat[2]) < 0.0f);
1442 }
1443
1444 bool is_negative_m4(float mat[4][4])
1445 {
1446         float vec[3];
1447         cross_v3_v3v3(vec, mat[0], mat[1]);
1448         return (dot_v3v3(vec, mat[2]) < 0.0f);
1449 }
1450
1451 bool is_zero_m3(float mat[3][3])
1452 {
1453         return (is_zero_v3(mat[0]) &&
1454                 is_zero_v3(mat[1]) &&
1455                 is_zero_v3(mat[2]));
1456 }
1457 bool is_zero_m4(float mat[4][4])
1458 {
1459         return (is_zero_v4(mat[0]) &&
1460                 is_zero_v4(mat[1]) &&
1461                 is_zero_v4(mat[2]) &&
1462                 is_zero_v4(mat[3]));
1463 }
1464
1465 /* make a 4x4 matrix out of 3 transform components */
1466 /* matrices are made in the order: scale * rot * loc */
1467 /* TODO: need to have a version that allows for rotation order... */
1468
1469 void loc_eul_size_to_mat4(float mat[4][4], const float loc[3], const float eul[3], const float size[3])
1470 {
1471         float rmat[3][3], smat[3][3], tmat[3][3];
1472
1473         /* initialize new matrix */
1474         unit_m4(mat);
1475
1476         /* make rotation + scaling part */
1477         eul_to_mat3(rmat, eul);
1478         size_to_mat3(smat, size);
1479         mul_m3_m3m3(tmat, rmat, smat);
1480
1481         /* copy rot/scale part to output matrix*/
1482         copy_m4_m3(mat, tmat);
1483
1484         /* copy location to matrix */
1485         mat[3][0] = loc[0];
1486         mat[3][1] = loc[1];
1487         mat[3][2] = loc[2];
1488 }
1489
1490 /* make a 4x4 matrix out of 3 transform components */
1491
1492 /* matrices are made in the order: scale * rot * loc */
1493 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)
1494 {
1495         float rmat[3][3], smat[3][3], tmat[3][3];
1496
1497         /* initialize new matrix */
1498         unit_m4(mat);
1499
1500         /* make rotation + scaling part */
1501         eulO_to_mat3(rmat, eul, rotOrder);
1502         size_to_mat3(smat, size);
1503         mul_m3_m3m3(tmat, rmat, smat);
1504
1505         /* copy rot/scale part to output matrix*/
1506         copy_m4_m3(mat, tmat);
1507
1508         /* copy location to matrix */
1509         mat[3][0] = loc[0];
1510         mat[3][1] = loc[1];
1511         mat[3][2] = loc[2];
1512 }
1513
1514
1515 /* make a 4x4 matrix out of 3 transform components */
1516
1517 /* matrices are made in the order: scale * rot * loc */
1518 void loc_quat_size_to_mat4(float mat[4][4], const float loc[3], const float quat[4], const float size[3])
1519 {
1520         float rmat[3][3], smat[3][3], tmat[3][3];
1521
1522         /* initialize new matrix */
1523         unit_m4(mat);
1524
1525         /* make rotation + scaling part */
1526         quat_to_mat3(rmat, quat);
1527         size_to_mat3(smat, size);
1528         mul_m3_m3m3(tmat, rmat, smat);
1529
1530         /* copy rot/scale part to output matrix*/
1531         copy_m4_m3(mat, tmat);
1532
1533         /* copy location to matrix */
1534         mat[3][0] = loc[0];
1535         mat[3][1] = loc[1];
1536         mat[3][2] = loc[2];
1537 }
1538
1539 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])
1540 {
1541         float q[4];
1542         axis_angle_to_quat(q, axis, angle);
1543         loc_quat_size_to_mat4(mat, loc, q, size);
1544 }
1545
1546 /*********************************** Other ***********************************/
1547
1548 void print_m3(const char *str, float m[3][3])
1549 {
1550         printf("%s\n", str);
1551         printf("%f %f %f\n", m[0][0], m[1][0], m[2][0]);
1552         printf("%f %f %f\n", m[0][1], m[1][1], m[2][1]);
1553         printf("%f %f %f\n", m[0][2], m[1][2], m[2][2]);
1554         printf("\n");
1555 }
1556
1557 void print_m4(const char *str, float m[4][4])
1558 {
1559         printf("%s\n", str);
1560         printf("%f %f %f %f\n", m[0][0], m[1][0], m[2][0], m[3][0]);
1561         printf("%f %f %f %f\n", m[0][1], m[1][1], m[2][1], m[3][1]);
1562         printf("%f %f %f %f\n", m[0][2], m[1][2], m[2][2], m[3][2]);
1563         printf("%f %f %f %f\n", m[0][3], m[1][3], m[2][3], m[3][3]);
1564         printf("\n");
1565 }
1566
1567 /*********************************** SVD ************************************
1568  * from TNT matrix library
1569  *
1570  * Compute the Single Value Decomposition of an arbitrary matrix A
1571  * That is compute the 3 matrices U,W,V with U column orthogonal (m,n)
1572  * ,W a diagonal matrix and V an orthogonal square matrix s.t.
1573  * A = U.W.Vt. From this decomposition it is trivial to compute the
1574  * (pseudo-inverse) of A as Ainv = V.Winv.tranpose(U).
1575  */
1576
1577 void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4])
1578 {
1579         float A[4][4];
1580         float work1[4], work2[4];
1581         int m = 4;
1582         int n = 4;
1583         int maxiter = 200;
1584         int nu = min_ff(m, n);
1585
1586         float *work = work1;
1587         float *e = work2;
1588         float eps;
1589
1590         int i = 0, j = 0, k = 0, p, pp, iter;
1591
1592         /* Reduce A to bidiagonal form, storing the diagonal elements
1593          * in s and the super-diagonal elements in e. */
1594
1595         int nct = min_ff(m - 1, n);
1596         int nrt = max_ff(0, min_ff(n - 2, m));
1597
1598         copy_m4_m4(A, A_);
1599         zero_m4(U);
1600         zero_v4(s);
1601
1602         for (k = 0; k < max_ff(nct, nrt); k++) {
1603                 if (k < nct) {
1604
1605                         /* Compute the transformation for the k-th column and
1606                          * place the k-th diagonal in s[k].
1607                          * Compute 2-norm of k-th column without under/overflow. */
1608                         s[k] = 0;
1609                         for (i = k; i < m; i++) {
1610                                 s[k] = hypotf(s[k], A[i][k]);
1611                         }
1612                         if (s[k] != 0.0f) {
1613                                 float invsk;
1614                                 if (A[k][k] < 0.0f) {
1615                                         s[k] = -s[k];
1616                                 }
1617                                 invsk = 1.0f / s[k];
1618                                 for (i = k; i < m; i++) {
1619                                         A[i][k] *= invsk;
1620                                 }
1621                                 A[k][k] += 1.0f;
1622                         }
1623                         s[k] = -s[k];
1624                 }
1625                 for (j = k + 1; j < n; j++) {
1626                         if ((k < nct) && (s[k] != 0.0f)) {
1627
1628                                 /* Apply the transformation. */
1629
1630                                 float t = 0;
1631                                 for (i = k; i < m; i++) {
1632                                         t += A[i][k] * A[i][j];
1633                                 }
1634                                 t = -t / A[k][k];
1635                                 for (i = k; i < m; i++) {
1636                                         A[i][j] += t * A[i][k];
1637                                 }
1638                         }
1639
1640                         /* Place the k-th row of A into e for the */
1641                         /* subsequent calculation of the row transformation. */
1642
1643                         e[j] = A[k][j];
1644                 }
1645                 if (k < nct) {
1646
1647                         /* Place the transformation in U for subsequent back
1648                          * multiplication. */
1649
1650                         for (i = k; i < m; i++)
1651                                 U[i][k] = A[i][k];
1652                 }
1653                 if (k < nrt) {
1654
1655                         /* Compute the k-th row transformation and place the
1656                          * k-th super-diagonal in e[k].
1657                          * Compute 2-norm without under/overflow. */
1658                         e[k] = 0;
1659                         for (i = k + 1; i < n; i++) {
1660                                 e[k] = hypotf(e[k], e[i]);
1661                         }
1662                         if (e[k] != 0.0f) {
1663                                 float invek;
1664                                 if (e[k + 1] < 0.0f) {
1665                                         e[k] = -e[k];
1666                                 }
1667                                 invek = 1.0f / e[k];
1668                                 for (i = k + 1; i < n; i++) {
1669                                         e[i] *= invek;
1670                                 }
1671                                 e[k + 1] += 1.0f;
1672                         }
1673                         e[k] = -e[k];
1674                         if ((k + 1 < m) & (e[k] != 0.0f)) {
1675                                 float invek1;
1676
1677                                 /* Apply the transformation. */
1678
1679                                 for (i = k + 1; i < m; i++) {
1680                                         work[i] = 0.0f;
1681                                 }
1682                                 for (j = k + 1; j < n; j++) {
1683                                         for (i = k + 1; i < m; i++) {
1684                                                 work[i] += e[j] * A[i][j];
1685                                         }
1686                                 }
1687                                 invek1 = 1.0f / e[k + 1];
1688                                 for (j = k + 1; j < n; j++) {
1689                                         float t = -e[j] * invek1;
1690                                         for (i = k + 1; i < m; i++) {
1691                                                 A[i][j] += t * work[i];
1692                                         }
1693                                 }
1694                         }
1695
1696                         /* Place the transformation in V for subsequent
1697                          * back multiplication. */
1698
1699                         for (i = k + 1; i < n; i++)
1700                                 V[i][k] = e[i];
1701                 }
1702         }
1703
1704         /* Set up the final bidiagonal matrix or order p. */
1705
1706         p = min_ff(n, m + 1);
1707         if (nct < n) {
1708                 s[nct] = A[nct][nct];
1709         }
1710         if (m < p) {
1711                 s[p - 1] = 0.0f;
1712         }
1713         if (nrt + 1 < p) {
1714                 e[nrt] = A[nrt][p - 1];
1715         }
1716         e[p - 1] = 0.0f;
1717
1718         /* If required, generate U. */
1719
1720         for (j = nct; j < nu; j++) {
1721                 for (i = 0; i < m; i++) {
1722                         U[i][j] = 0.0f;
1723                 }
1724                 U[j][j] = 1.0f;
1725         }
1726         for (k = nct - 1; k >= 0; k--) {
1727                 if (s[k] != 0.0f) {
1728                         for (j = k + 1; j < nu; j++) {
1729                                 float t = 0;
1730                                 for (i = k; i < m; i++) {
1731                                         t += U[i][k] * U[i][j];
1732                                 }
1733                                 t = -t / U[k][k];
1734                                 for (i = k; i < m; i++) {
1735                                         U[i][j] += t * U[i][k];
1736                                 }
1737                         }
1738                         for (i = k; i < m; i++) {
1739                                 U[i][k] = -U[i][k];
1740                         }
1741                         U[k][k] = 1.0f + U[k][k];
1742                         for (i = 0; i < k - 1; i++) {
1743                                 U[i][k] = 0.0f;
1744                         }
1745                 }
1746                 else {
1747                         for (i = 0; i < m; i++) {
1748                                 U[i][k] = 0.0f;
1749                         }
1750                         U[k][k] = 1.0f;
1751                 }
1752         }
1753
1754         /* If required, generate V. */
1755
1756         for (k = n - 1; k >= 0; k--) {
1757                 if ((k < nrt) & (e[k] != 0.0f)) {
1758                         for (j = k + 1; j < nu; j++) {
1759                                 float t = 0;
1760                                 for (i = k + 1; i < n; i++) {
1761                                         t += V[i][k] * V[i][j];
1762                                 }
1763                                 t = -t / V[k + 1][k];
1764                                 for (i = k + 1; i < n; i++) {
1765                                         V[i][j] += t * V[i][k];
1766                                 }
1767                         }
1768                 }
1769                 for (i = 0; i < n; i++) {
1770                         V[i][k] = 0.0f;
1771                 }
1772                 V[k][k] = 1.0f;
1773         }
1774
1775         /* Main iteration loop for the singular values. */
1776
1777         pp = p - 1;
1778         iter = 0;
1779         eps = powf(2.0f, -52.0f);
1780         while (p > 0) {
1781                 int kase = 0;
1782
1783                 /* Test for maximum iterations to avoid infinite loop */
1784                 if (maxiter == 0)
1785                         break;
1786                 maxiter--;
1787
1788                 /* This section of the program inspects for
1789                  * negligible elements in the s and e arrays.  On
1790                  * completion the variables kase and k are set as follows.
1791                  *
1792                  * kase = 1       if s(p) and e[k - 1] are negligible and k<p
1793                  * kase = 2       if s(k) is negligible and k<p
1794                  * kase = 3       if e[k - 1] is negligible, k<p, and
1795                  *               s(k), ..., s(p) are not negligible (qr step).
1796                  * kase = 4       if e(p - 1) is negligible (convergence). */
1797
1798                 for (k = p - 2; k >= -1; k--) {
1799                         if (k == -1) {
1800                                 break;
1801                         }
1802                         if (fabsf(e[k]) <= eps * (fabsf(s[k]) + fabsf(s[k + 1]))) {
1803                                 e[k] = 0.0f;
1804                                 break;
1805                         }
1806                 }
1807                 if (k == p - 2) {
1808                         kase = 4;
1809                 }
1810                 else {
1811                         int ks;
1812                         for (ks = p - 1; ks >= k; ks--) {
1813                                 float t;
1814                                 if (ks == k) {
1815                                         break;
1816                                 }
1817                                 t = (ks != p ? fabsf(e[ks]) : 0.f) +
1818                                     (ks != k + 1 ? fabsf(e[ks - 1]) : 0.0f);
1819                                 if (fabsf(s[ks]) <= eps * t) {
1820                                         s[ks] = 0.0f;
1821                                         break;
1822                                 }
1823                         }
1824                         if (ks == k) {
1825                                 kase = 3;
1826                         }
1827                         else if (ks == p - 1) {
1828                                 kase = 1;
1829                         }
1830                         else {
1831                                 kase = 2;
1832                                 k = ks;
1833                         }
1834                 }
1835                 k++;
1836
1837                 /* Perform the task indicated by kase. */
1838
1839                 switch (kase) {
1840
1841                         /* Deflate negligible s(p). */
1842
1843                         case 1:
1844                         {
1845                                 float f = e[p - 2];
1846                                 e[p - 2] = 0.0f;
1847                                 for (j = p - 2; j >= k; j--) {
1848                                         float t = hypotf(s[j], f);
1849                                         float invt = 1.0f / t;
1850                                         float cs = s[j] * invt;
1851                                         float sn = f * invt;
1852                                         s[j] = t;
1853                                         if (j != k) {
1854                                                 f = -sn * e[j - 1];
1855                                                 e[j - 1] = cs * e[j - 1];
1856                                         }
1857
1858                                         for (i = 0; i < n; i++) {
1859                                                 t = cs * V[i][j] + sn * V[i][p - 1];
1860                                                 V[i][p - 1] = -sn * V[i][j] + cs * V[i][p - 1];
1861                                                 V[i][j] = t;
1862                                         }
1863                                 }
1864                                 break;
1865                         }
1866
1867                         /* Split at negligible s(k). */
1868
1869                         case 2:
1870                         {
1871                                 float f = e[k - 1];
1872                                 e[k - 1] = 0.0f;
1873                                 for (j = k; j < p; j++) {
1874                                         float t = hypotf(s[j], f);
1875                                         float invt = 1.0f / t;
1876                                         float cs = s[j] * invt;
1877                                         float sn = f * invt;
1878                                         s[j] = t;
1879                                         f = -sn * e[j];
1880                                         e[j] = cs * e[j];
1881
1882                                         for (i = 0; i < m; i++) {
1883                                                 t = cs * U[i][j] + sn * U[i][k - 1];
1884                                                 U[i][k - 1] = -sn * U[i][j] + cs * U[i][k - 1];
1885                                                 U[i][j] = t;
1886                                         }
1887                                 }
1888                                 break;
1889                         }
1890
1891                         /* Perform one qr step. */
1892
1893                         case 3:
1894                         {
1895
1896                                 /* Calculate the shift. */
1897
1898                                 float scale = max_ff(max_ff(max_ff(max_ff(
1899                                                    fabsf(s[p - 1]), fabsf(s[p - 2])), fabsf(e[p - 2])),
1900                                                    fabsf(s[k])), fabsf(e[k]));
1901                                 float invscale = 1.0f / scale;
1902                                 float sp = s[p - 1] * invscale;
1903                                 float spm1 = s[p - 2] * invscale;
1904                                 float epm1 = e[p - 2] * invscale;
1905                                 float sk = s[k] * invscale;
1906                                 float ek = e[k] * invscale;
1907                                 float b = ((spm1 + sp) * (spm1 - sp) + epm1 * epm1) * 0.5f;
1908                                 float c = (sp * epm1) * (sp * epm1);
1909                                 float shift = 0.0f;
1910                                 float f, g;
1911                                 if ((b != 0.0f) || (c != 0.0f)) {
1912                                         shift = sqrtf(b * b + c);
1913                                         if (b < 0.0f) {
1914                                                 shift = -shift;
1915                                         }
1916                                         shift = c / (b + shift);
1917                                 }
1918                                 f = (sk + sp) * (sk - sp) + shift;
1919                                 g = sk * ek;
1920
1921                                 /* Chase zeros. */
1922
1923                                 for (j = k; j < p - 1; j++) {
1924                                         float t = hypotf(f, g);
1925                                         /* division by zero checks added to avoid NaN (brecht) */
1926                                         float cs = (t == 0.0f) ? 0.0f : f / t;
1927                                         float sn = (t == 0.0f) ? 0.0f : g / t;
1928                                         if (j != k) {
1929                                                 e[j - 1] = t;
1930                                         }
1931                                         f = cs * s[j] + sn * e[j];
1932                                         e[j] = cs * e[j] - sn * s[j];
1933                                         g = sn * s[j + 1];
1934                                         s[j + 1] = cs * s[j + 1];
1935
1936                                         for (i = 0; i < n; i++) {
1937                                                 t = cs * V[i][j] + sn * V[i][j + 1];
1938                                                 V[i][j + 1] = -sn * V[i][j] + cs * V[i][j + 1];
1939                                                 V[i][j] = t;
1940                                         }
1941
1942                                         t = hypotf(f, g);
1943                                         /* division by zero checks added to avoid NaN (brecht) */
1944                                         cs = (t == 0.0f) ? 0.0f : f / t;
1945                                         sn = (t == 0.0f) ? 0.0f : g / t;
1946                                         s[j] = t;
1947                                         f = cs * e[j] + sn * s[j + 1];
1948                                         s[j + 1] = -sn * e[j] + cs * s[j + 1];
1949                                         g = sn * e[j + 1];
1950                                         e[j + 1] = cs * e[j + 1];
1951                                         if (j < m - 1) {
1952                                                 for (i = 0; i < m; i++) {
1953                                                         t = cs * U[i][j] + sn * U[i][j + 1];
1954                                                         U[i][j + 1] = -sn * U[i][j] + cs * U[i][j + 1];
1955                                                         U[i][j] = t;
1956                                                 }
1957                                         }
1958                                 }
1959                                 e[p - 2] = f;
1960                                 iter = iter + 1;
1961                                 break;
1962                         }
1963                         /* Convergence. */
1964
1965                         case 4:
1966                         {
1967
1968                                 /* Make the singular values positive. */
1969
1970                                 if (s[k] <= 0.0f) {
1971                                         s[k] = (s[k] < 0.0f ? -s[k] : 0.0f);
1972
1973                                         for (i = 0; i <= pp; i++)
1974                                                 V[i][k] = -V[i][k];
1975                                 }
1976
1977                                 /* Order the singular values. */
1978
1979                                 while (k < pp) {
1980                                         float t;
1981                                         if (s[k] >= s[k + 1]) {
1982                                                 break;
1983                                         }
1984                                         t = s[k];
1985                                         s[k] = s[k + 1];
1986                                         s[k + 1] = t;
1987                                         if (k < n - 1) {
1988                                                 for (i = 0; i < n; i++) {
1989                                                         t = V[i][k + 1];
1990                                                         V[i][k + 1] = V[i][k];
1991                                                         V[i][k] = t;
1992                                                 }
1993                                         }
1994                                         if (k < m - 1) {
1995                                                 for (i = 0; i < m; i++) {
1996                                                         t = U[i][k + 1];
1997                                                         U[i][k + 1] = U[i][k];
1998                                                         U[i][k] = t;
1999                                                 }
2000                                         }
2001                                         k++;
2002                                 }
2003                                 iter = 0;
2004                                 p--;
2005                                 break;
2006                         }
2007                 }
2008         }
2009 }
2010
2011 void pseudoinverse_m4_m4(float Ainv[4][4], float A[4][4], float epsilon)
2012 {
2013         /* compute moon-penrose pseudo inverse of matrix, singular values
2014          * below epsilon are ignored for stability (truncated SVD) */
2015         float V[4][4], W[4], Wm[4][4], U[4][4];
2016         int i;
2017
2018         transpose_m4(A);
2019         svd_m4(V, W, U, A);
2020         transpose_m4(U);
2021         transpose_m4(V);
2022
2023         zero_m4(Wm);
2024         for (i = 0; i < 4; i++)
2025                 Wm[i][i] = (W[i] < epsilon) ? 0.0f : 1.0f / W[i];
2026
2027         transpose_m4(V);
2028
2029         mul_serie_m4(Ainv, U, Wm, V, NULL, NULL, NULL, NULL, NULL);
2030 }
2031
2032 void pseudoinverse_m3_m3(float Ainv[3][3], float A[3][3], float epsilon)
2033 {
2034         /* try regular inverse when possible, otherwise fall back to slow svd */
2035         if (!invert_m3_m3(Ainv, A)) {
2036                 float tmp[4][4], tmpinv[4][4];
2037
2038                 copy_m4_m3(tmp, A);
2039                 pseudoinverse_m4_m4(tmpinv, tmp, epsilon);
2040                 copy_m3_m4(Ainv, tmpinv);
2041         }
2042 }
2043