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