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