115980cb3e63cf53b9346047cf5468b8885299eb
[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_m4(float mat[4][4])
880 {
881         float t;
882
883         t = mat[0][1];
884         mat[0][1] = mat[1][0];
885         mat[1][0] = t;
886         t = mat[0][2];
887         mat[0][2] = mat[2][0];
888         mat[2][0] = t;
889         t = mat[0][3];
890         mat[0][3] = mat[3][0];
891         mat[3][0] = t;
892
893         t = mat[1][2];
894         mat[1][2] = mat[2][1];
895         mat[2][1] = t;
896         t = mat[1][3];
897         mat[1][3] = mat[3][1];
898         mat[3][1] = t;
899
900         t = mat[2][3];
901         mat[2][3] = mat[3][2];
902         mat[3][2] = t;
903 }
904
905 int compare_m4m4(float mat1[4][4], float mat2[4][4], float limit)
906 {
907         if (compare_v4v4(mat1[0], mat2[0], limit))
908                 if (compare_v4v4(mat1[1], mat2[1], limit))
909                         if (compare_v4v4(mat1[2], mat2[2], limit))
910                                 if (compare_v4v4(mat1[3], mat2[3], limit))
911                                         return 1;
912         return 0;
913 }
914
915 void orthogonalize_m3(float mat[3][3], int axis)
916 {
917         float size[3];
918         mat3_to_size(size, mat);
919         normalize_v3(mat[axis]);
920         switch (axis) {
921                 case 0:
922                         if (dot_v3v3(mat[0], mat[1]) < 1) {
923                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
924                                 normalize_v3(mat[2]);
925                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
926                         }
927                         else if (dot_v3v3(mat[0], mat[2]) < 1) {
928                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
929                                 normalize_v3(mat[1]);
930                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
931                         }
932                         else {
933                                 float vec[3];
934
935                                 vec[0] = mat[0][1];
936                                 vec[1] = mat[0][2];
937                                 vec[2] = mat[0][0];
938
939                                 cross_v3_v3v3(mat[2], mat[0], vec);
940                                 normalize_v3(mat[2]);
941                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
942                         }
943                         break;
944                 case 1:
945                         if (dot_v3v3(mat[1], mat[0]) < 1) {
946                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
947                                 normalize_v3(mat[2]);
948                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
949                         }
950                         else if (dot_v3v3(mat[0], mat[2]) < 1) {
951                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
952                                 normalize_v3(mat[0]);
953                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
954                         }
955                         else {
956                                 float vec[3];
957
958                                 vec[0] = mat[1][1];
959                                 vec[1] = mat[1][2];
960                                 vec[2] = mat[1][0];
961
962                                 cross_v3_v3v3(mat[0], mat[1], vec);
963                                 normalize_v3(mat[0]);
964                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
965                         }
966                         break;
967                 case 2:
968                         if (dot_v3v3(mat[2], mat[0]) < 1) {
969                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
970                                 normalize_v3(mat[1]);
971                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
972                         }
973                         else if (dot_v3v3(mat[2], mat[1]) < 1) {
974                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
975                                 normalize_v3(mat[0]);
976                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
977                         }
978                         else {
979                                 float vec[3];
980
981                                 vec[0] = mat[2][1];
982                                 vec[1] = mat[2][2];
983                                 vec[2] = mat[2][0];
984
985                                 cross_v3_v3v3(mat[0], vec, mat[2]);
986                                 normalize_v3(mat[0]);
987                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
988                         }
989                         break;
990                 default:
991                         BLI_assert(0);
992                         break;
993         }
994         mul_v3_fl(mat[0], size[0]);
995         mul_v3_fl(mat[1], size[1]);
996         mul_v3_fl(mat[2], size[2]);
997 }
998
999 void orthogonalize_m4(float mat[4][4], int axis)
1000 {
1001         float size[3];
1002         mat4_to_size(size, mat);
1003         normalize_v3(mat[axis]);
1004         switch (axis) {
1005                 case 0:
1006                         if (dot_v3v3(mat[0], mat[1]) < 1) {
1007                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
1008                                 normalize_v3(mat[2]);
1009                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
1010                         }
1011                         else if (dot_v3v3(mat[0], mat[2]) < 1) {
1012                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
1013                                 normalize_v3(mat[1]);
1014                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
1015                         }
1016                         else {
1017                                 float vec[3];
1018
1019                                 vec[0] = mat[0][1];
1020                                 vec[1] = mat[0][2];
1021                                 vec[2] = mat[0][0];
1022
1023                                 cross_v3_v3v3(mat[2], mat[0], vec);
1024                                 normalize_v3(mat[2]);
1025                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
1026                         }
1027                         break;
1028                 case 1:
1029                         if (dot_v3v3(mat[1], mat[0]) < 1) {
1030                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
1031                                 normalize_v3(mat[2]);
1032                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
1033                         }
1034                         else if (dot_v3v3(mat[0], mat[2]) < 1) {
1035                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
1036                                 normalize_v3(mat[0]);
1037                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
1038                         }
1039                         else {
1040                                 float vec[3];
1041
1042                                 vec[0] = mat[1][1];
1043                                 vec[1] = mat[1][2];
1044                                 vec[2] = mat[1][0];
1045
1046                                 cross_v3_v3v3(mat[0], mat[1], vec);
1047                                 normalize_v3(mat[0]);
1048                                 cross_v3_v3v3(mat[2], mat[0], mat[1]);
1049                         }
1050                         break;
1051                 case 2:
1052                         if (dot_v3v3(mat[2], mat[0]) < 1) {
1053                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
1054                                 normalize_v3(mat[1]);
1055                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
1056                         }
1057                         else if (dot_v3v3(mat[2], mat[1]) < 1) {
1058                                 cross_v3_v3v3(mat[0], mat[1], mat[2]);
1059                                 normalize_v3(mat[0]);
1060                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
1061                         }
1062                         else {
1063                                 float vec[3];
1064
1065                                 vec[0] = mat[2][1];
1066                                 vec[1] = mat[2][2];
1067                                 vec[2] = mat[2][0];
1068
1069                                 cross_v3_v3v3(mat[0], vec, mat[2]);
1070                                 normalize_v3(mat[0]);
1071                                 cross_v3_v3v3(mat[1], mat[2], mat[0]);
1072                         }
1073                         break;
1074                 default:
1075                         BLI_assert(0);
1076                         break;
1077         }
1078         mul_v3_fl(mat[0], size[0]);
1079         mul_v3_fl(mat[1], size[1]);
1080         mul_v3_fl(mat[2], size[2]);
1081 }
1082
1083 bool is_orthogonal_m3(float m[3][3])
1084 {
1085         int i, j;
1086
1087         for (i = 0; i < 3; i++) {
1088                 for (j = 0; j < i; j++) {
1089                         if (fabsf(dot_v3v3(m[i], m[j])) > 1.5f * FLT_EPSILON)
1090                                 return 0;
1091                 }
1092         }
1093
1094         return 1;
1095 }
1096
1097 bool is_orthogonal_m4(float m[4][4])
1098 {
1099         int i, j;
1100
1101         for (i = 0; i < 4; i++) {
1102                 for (j = 0; j < i; j++) {
1103                         if (fabsf(dot_v4v4(m[i], m[j])) > 1.5f * FLT_EPSILON)
1104                                 return 0;
1105                 }
1106
1107         }
1108
1109         return 1;
1110 }
1111
1112 bool is_orthonormal_m3(float m[3][3])
1113 {
1114         if (is_orthogonal_m3(m)) {
1115                 int i;
1116
1117                 for (i = 0; i < 3; i++)
1118                         if (fabsf(dot_v3v3(m[i], m[i]) - 1) > 1.5f * FLT_EPSILON)
1119                                 return 0;
1120
1121                 return 1;
1122         }
1123
1124         return 0;
1125 }
1126
1127 bool is_orthonormal_m4(float m[4][4])
1128 {
1129         if (is_orthogonal_m4(m)) {
1130                 int i;
1131
1132                 for (i = 0; i < 4; i++)
1133                         if (fabsf(dot_v4v4(m[i], m[i]) - 1) > 1.5f * FLT_EPSILON)
1134                                 return 0;
1135
1136                 return 1;
1137         }
1138
1139         return 0;
1140 }
1141
1142 bool is_uniform_scaled_m3(float m[3][3])
1143 {
1144         const float eps = 1e-7f;
1145         float t[3][3];
1146         float l1, l2, l3, l4, l5, l6;
1147
1148         copy_m3_m3(t, m);
1149         transpose_m3(t);
1150
1151         l1 = len_squared_v3(m[0]);
1152         l2 = len_squared_v3(m[1]);
1153         l3 = len_squared_v3(m[2]);
1154
1155         l4 = len_squared_v3(t[0]);
1156         l5 = len_squared_v3(t[1]);
1157         l6 = len_squared_v3(t[2]);
1158
1159         if (fabsf(l2 - l1) <= eps &&
1160             fabsf(l3 - l1) <= eps &&
1161             fabsf(l4 - l1) <= eps &&
1162             fabsf(l5 - l1) <= eps &&
1163             fabsf(l6 - l1) <= eps)
1164         {
1165                 return true;
1166         }
1167
1168         return false;
1169 }
1170
1171 bool is_uniform_scaled_m4(float m[4][4])
1172 {
1173         float t[3][3];
1174         copy_m3_m4(t, m);
1175         return is_uniform_scaled_m3(t);
1176 }
1177
1178 void normalize_m3(float mat[3][3])
1179 {
1180         normalize_v3(mat[0]);
1181         normalize_v3(mat[1]);
1182         normalize_v3(mat[2]);
1183 }
1184
1185 void normalize_m3_m3(float rmat[3][3], float mat[3][3])
1186 {
1187         normalize_v3_v3(rmat[0], mat[0]);
1188         normalize_v3_v3(rmat[1], mat[1]);
1189         normalize_v3_v3(rmat[2], mat[2]);
1190 }
1191
1192 void normalize_m4(float mat[4][4])
1193 {
1194         float len;
1195
1196         len = normalize_v3(mat[0]);
1197         if (len != 0.0f) mat[0][3] /= len;
1198         len = normalize_v3(mat[1]);
1199         if (len != 0.0f) mat[1][3] /= len;
1200         len = normalize_v3(mat[2]);
1201         if (len != 0.0f) mat[2][3] /= len;
1202 }
1203
1204 void normalize_m4_m4(float rmat[4][4], float mat[4][4])
1205 {
1206         copy_m4_m4(rmat, mat);
1207         normalize_m4(rmat);
1208 }
1209
1210 void adjoint_m2_m2(float m1[2][2], float m[2][2])
1211 {
1212         BLI_assert(m1 != m);
1213         m1[0][0] =  m[1][1];
1214         m1[0][1] = -m[0][1];
1215         m1[1][0] = -m[1][0];
1216         m1[1][1] =  m[0][0];
1217 }
1218
1219 void adjoint_m3_m3(float m1[3][3], float m[3][3])
1220 {
1221         BLI_assert(m1 != m);
1222         m1[0][0] = m[1][1] * m[2][2] - m[1][2] * m[2][1];
1223         m1[0][1] = -m[0][1] * m[2][2] + m[0][2] * m[2][1];
1224         m1[0][2] = m[0][1] * m[1][2] - m[0][2] * m[1][1];
1225
1226         m1[1][0] = -m[1][0] * m[2][2] + m[1][2] * m[2][0];
1227         m1[1][1] = m[0][0] * m[2][2] - m[0][2] * m[2][0];
1228         m1[1][2] = -m[0][0] * m[1][2] + m[0][2] * m[1][0];
1229
1230         m1[2][0] = m[1][0] * m[2][1] - m[1][1] * m[2][0];
1231         m1[2][1] = -m[0][0] * m[2][1] + m[0][1] * m[2][0];
1232         m1[2][2] = m[0][0] * m[1][1] - m[0][1] * m[1][0];
1233 }
1234
1235 void adjoint_m4_m4(float out[4][4], float in[4][4]) /* out = ADJ(in) */
1236 {
1237         float a1, a2, a3, a4, b1, b2, b3, b4;
1238         float c1, c2, c3, c4, d1, d2, d3, d4;
1239
1240         a1 = in[0][0];
1241         b1 = in[0][1];
1242         c1 = in[0][2];
1243         d1 = in[0][3];
1244
1245         a2 = in[1][0];
1246         b2 = in[1][1];
1247         c2 = in[1][2];
1248         d2 = in[1][3];
1249
1250         a3 = in[2][0];
1251         b3 = in[2][1];
1252         c3 = in[2][2];
1253         d3 = in[2][3];
1254
1255         a4 = in[3][0];
1256         b4 = in[3][1];
1257         c4 = in[3][2];
1258         d4 = in[3][3];
1259
1260
1261         out[0][0] = determinant_m3(b2, b3, b4, c2, c3, c4, d2, d3, d4);
1262         out[1][0] = -determinant_m3(a2, a3, a4, c2, c3, c4, d2, d3, d4);
1263         out[2][0] = determinant_m3(a2, a3, a4, b2, b3, b4, d2, d3, d4);
1264         out[3][0] = -determinant_m3(a2, a3, a4, b2, b3, b4, c2, c3, c4);
1265
1266         out[0][1] = -determinant_m3(b1, b3, b4, c1, c3, c4, d1, d3, d4);
1267         out[1][1] = determinant_m3(a1, a3, a4, c1, c3, c4, d1, d3, d4);
1268         out[2][1] = -determinant_m3(a1, a3, a4, b1, b3, b4, d1, d3, d4);
1269         out[3][1] = determinant_m3(a1, a3, a4, b1, b3, b4, c1, c3, c4);
1270
1271         out[0][2] = determinant_m3(b1, b2, b4, c1, c2, c4, d1, d2, d4);
1272         out[1][2] = -determinant_m3(a1, a2, a4, c1, c2, c4, d1, d2, d4);
1273         out[2][2] = determinant_m3(a1, a2, a4, b1, b2, b4, d1, d2, d4);
1274         out[3][2] = -determinant_m3(a1, a2, a4, b1, b2, b4, c1, c2, c4);
1275
1276         out[0][3] = -determinant_m3(b1, b2, b3, c1, c2, c3, d1, d2, d3);
1277         out[1][3] = determinant_m3(a1, a2, a3, c1, c2, c3, d1, d2, d3);
1278         out[2][3] = -determinant_m3(a1, a2, a3, b1, b2, b3, d1, d2, d3);
1279         out[3][3] = determinant_m3(a1, a2, a3, b1, b2, b3, c1, c2, c3);
1280 }
1281
1282 float determinant_m2(float a, float b, float c, float d)
1283 {
1284
1285         return a * d - b * c;
1286 }
1287
1288 float determinant_m3(float a1, float a2, float a3,
1289                      float b1, float b2, float b3,
1290                      float c1, float c2, float c3)
1291 {
1292         float ans;
1293
1294         ans = (a1 * determinant_m2(b2, b3, c2, c3) -
1295                b1 * determinant_m2(a2, a3, c2, c3) +
1296                c1 * determinant_m2(a2, a3, b2, b3));
1297
1298         return ans;
1299 }
1300
1301 float determinant_m4(float m[4][4])
1302 {
1303         float ans;
1304         float a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4;
1305
1306         a1 = m[0][0];
1307         b1 = m[0][1];
1308         c1 = m[0][2];
1309         d1 = m[0][3];
1310
1311         a2 = m[1][0];
1312         b2 = m[1][1];
1313         c2 = m[1][2];
1314         d2 = m[1][3];
1315
1316         a3 = m[2][0];
1317         b3 = m[2][1];
1318         c3 = m[2][2];
1319         d3 = m[2][3];
1320
1321         a4 = m[3][0];
1322         b4 = m[3][1];
1323         c4 = m[3][2];
1324         d4 = m[3][3];
1325
1326         ans = (a1 * determinant_m3(b2, b3, b4, c2, c3, c4, d2, d3, d4) -
1327                b1 * determinant_m3(a2, a3, a4, c2, c3, c4, d2, d3, d4) +
1328                c1 * determinant_m3(a2, a3, a4, b2, b3, b4, d2, d3, d4) -
1329                d1 * determinant_m3(a2, a3, a4, b2, b3, b4, c2, c3, c4));
1330
1331         return ans;
1332 }
1333
1334 /****************************** Transformations ******************************/
1335
1336 void size_to_mat3(float mat[3][3], const float size[3])
1337 {
1338         mat[0][0] = size[0];
1339         mat[0][1] = 0.0f;
1340         mat[0][2] = 0.0f;
1341         mat[1][1] = size[1];
1342         mat[1][0] = 0.0f;
1343         mat[1][2] = 0.0f;
1344         mat[2][2] = size[2];
1345         mat[2][1] = 0.0f;
1346         mat[2][0] = 0.0f;
1347 }
1348
1349 void size_to_mat4(float mat[4][4], const float size[3])
1350 {
1351         mat[0][0] = size[0];
1352         mat[0][1] = 0.0f;
1353         mat[0][2] = 0.0f;
1354         mat[0][3] = 0.0f;
1355         mat[1][0] = 0.0f;
1356         mat[1][1] = size[1];
1357         mat[1][2] = 0.0f;
1358         mat[1][3] = 0.0f;
1359         mat[2][0] = 0.0f;
1360         mat[2][1] = 0.0f;
1361         mat[2][2] = size[2];
1362         mat[2][3] = 0.0f;
1363         mat[3][0] = 0.0f;
1364         mat[3][1] = 0.0f;
1365         mat[3][2] = 0.0f;
1366         mat[3][3] = 1.0f;
1367 }
1368
1369 void mat3_to_size(float size[3], float mat[3][3])
1370 {
1371         size[0] = len_v3(mat[0]);
1372         size[1] = len_v3(mat[1]);
1373         size[2] = len_v3(mat[2]);
1374 }
1375
1376 void mat4_to_size(float size[3], float mat[4][4])
1377 {
1378         size[0] = len_v3(mat[0]);
1379         size[1] = len_v3(mat[1]);
1380         size[2] = len_v3(mat[2]);
1381 }
1382
1383 /* this gets the average scale of a matrix, only use when your scaling
1384  * data that has no idea of scale axis, examples are bone-envelope-radius
1385  * and curve radius */
1386 float mat3_to_scale(float mat[3][3])
1387 {
1388         /* unit length vector */
1389         float unit_vec[3];
1390         copy_v3_fl(unit_vec, (float)(1.0 / M_SQRT3));
1391         mul_m3_v3(mat, unit_vec);
1392         return len_v3(unit_vec);
1393 }
1394
1395 float mat4_to_scale(float mat[4][4])
1396 {
1397         /* unit length vector */
1398         float unit_vec[3];
1399         copy_v3_fl(unit_vec, (float)(1.0 / M_SQRT3));
1400         mul_mat3_m4_v3(mat, unit_vec);
1401         return len_v3(unit_vec);
1402 }
1403
1404 void mat3_to_rot_size(float rot[3][3], float size[3], float mat3[3][3])
1405 {
1406         float mat3_n[3][3]; /* mat3 -> normalized, 3x3 */
1407         float imat3_n[3][3]; /* mat3 -> normalized & inverted, 3x3 */
1408
1409         /* rotation & scale are linked, we need to create the mat's
1410          * for these together since they are related. */
1411
1412         /* so scale doesn't interfere with rotation [#24291] */
1413         /* note: this is a workaround for negative matrix not working for rotation conversion, FIXME */
1414         normalize_m3_m3(mat3_n, mat3);
1415         if (is_negative_m3(mat3)) {
1416                 negate_v3(mat3_n[0]);
1417                 negate_v3(mat3_n[1]);
1418                 negate_v3(mat3_n[2]);
1419         }
1420
1421         /* rotation */
1422         /* keep rot as a 3x3 matrix, the caller can convert into a quat or euler */
1423         copy_m3_m3(rot, mat3_n);
1424
1425         /* scale */
1426         /* note: mat4_to_size(ob->size, mat) fails for negative scale */
1427         invert_m3_m3(imat3_n, mat3_n);
1428
1429         /* better not edit mat3 */
1430 #if 0
1431         mul_m3_m3m3(mat3, imat3_n, mat3);
1432
1433         size[0] = mat3[0][0];
1434         size[1] = mat3[1][1];
1435         size[2] = mat3[2][2];
1436 #else
1437         size[0] = dot_m3_v3_row_x(imat3_n, mat3[0]);
1438         size[1] = dot_m3_v3_row_y(imat3_n, mat3[1]);
1439         size[2] = dot_m3_v3_row_z(imat3_n, mat3[2]);
1440 #endif
1441 }
1442
1443 void mat4_to_loc_rot_size(float loc[3], float rot[3][3], float size[3], float wmat[4][4])
1444 {
1445         float mat3[3][3]; /* wmat -> 3x3 */
1446
1447         copy_m3_m4(mat3, wmat);
1448         mat3_to_rot_size(rot, size, mat3);
1449
1450         /* location */
1451         copy_v3_v3(loc, wmat[3]);
1452 }
1453
1454 void mat4_to_loc_quat(float loc[3], float quat[4], float wmat[4][4])
1455 {
1456         float mat3[3][3];
1457         float mat3_n[3][3]; /* normalized mat3 */
1458
1459         copy_m3_m4(mat3, wmat);
1460         normalize_m3_m3(mat3_n, mat3);
1461
1462         /* so scale doesn't interfere with rotation [#24291] */
1463         /* note: this is a workaround for negative matrix not working for rotation conversion, FIXME */
1464         if (is_negative_m3(mat3)) {
1465                 negate_v3(mat3_n[0]);
1466                 negate_v3(mat3_n[1]);
1467                 negate_v3(mat3_n[2]);
1468         }
1469
1470         mat3_to_quat(quat, mat3_n);
1471         copy_v3_v3(loc, wmat[3]);
1472 }
1473
1474 void mat4_decompose(float loc[3], float quat[4], float size[3], float wmat[4][4])
1475 {
1476         float rot[3][3];
1477         mat4_to_loc_rot_size(loc, rot, size, wmat);
1478         mat3_to_quat(quat, rot);
1479 }
1480
1481 void scale_m3_fl(float m[3][3], float scale)
1482 {
1483         m[0][0] = m[1][1] = m[2][2] = scale;
1484         m[0][1] = m[0][2] = 0.0;
1485         m[1][0] = m[1][2] = 0.0;
1486         m[2][0] = m[2][1] = 0.0;
1487 }
1488
1489 void scale_m4_fl(float m[4][4], float scale)
1490 {
1491         m[0][0] = m[1][1] = m[2][2] = scale;
1492         m[3][3] = 1.0;
1493         m[0][1] = m[0][2] = m[0][3] = 0.0;
1494         m[1][0] = m[1][2] = m[1][3] = 0.0;
1495         m[2][0] = m[2][1] = m[2][3] = 0.0;
1496         m[3][0] = m[3][1] = m[3][2] = 0.0;
1497 }
1498
1499 void translate_m4(float mat[4][4], float Tx, float Ty, float Tz)
1500 {
1501         mat[3][0] += (Tx * mat[0][0] + Ty * mat[1][0] + Tz * mat[2][0]);
1502         mat[3][1] += (Tx * mat[0][1] + Ty * mat[1][1] + Tz * mat[2][1]);
1503         mat[3][2] += (Tx * mat[0][2] + Ty * mat[1][2] + Tz * mat[2][2]);
1504 }
1505
1506 void rotate_m4(float mat[4][4], const char axis, const float angle)
1507 {
1508         int col;
1509         float temp[4] = {0.0f, 0.0f, 0.0f, 0.0f};
1510         float cosine, sine;
1511
1512         assert(axis >= 'X' && axis <= 'Z');
1513
1514         cosine = cosf(angle);
1515         sine   = sinf(angle);
1516         switch (axis) {
1517                 case 'X':
1518                         for (col = 0; col < 4; col++)
1519                                 temp[col] = cosine * mat[1][col] + sine * mat[2][col];
1520                         for (col = 0; col < 4; col++) {
1521                                 mat[2][col] = -sine * mat[1][col] + cosine * mat[2][col];
1522                                 mat[1][col] = temp[col];
1523                         }
1524                         break;
1525
1526                 case 'Y':
1527                         for (col = 0; col < 4; col++)
1528                                 temp[col] = cosine * mat[0][col] - sine * mat[2][col];
1529                         for (col = 0; col < 4; col++) {
1530                                 mat[2][col] = sine * mat[0][col] + cosine * mat[2][col];
1531                                 mat[0][col] = temp[col];
1532                         }
1533                         break;
1534
1535                 case 'Z':
1536                         for (col = 0; col < 4; col++)
1537                                 temp[col] = cosine * mat[0][col] + sine * mat[1][col];
1538                         for (col = 0; col < 4; col++) {
1539                                 mat[1][col] = -sine * mat[0][col] + cosine * mat[1][col];
1540                                 mat[0][col] = temp[col];
1541                         }
1542                         break;
1543         }
1544 }
1545
1546 void rotate_m2(float mat[2][2], const float angle)
1547 {
1548         mat[0][0] = mat[1][1] = cosf(angle);
1549         mat[0][1] = sinf(angle);
1550         mat[1][0] = -mat[0][1];
1551 }
1552
1553 /**
1554  * Scale or rotate around a pivot point,
1555  * a convenience function to avoid having to do inline.
1556  *
1557  * Since its common to make a scale/rotation matrix that pivots around an arbitrary point.
1558  *
1559  * Typical use case is to make 3x3 matrix, copy to 4x4, then pass to this function.
1560  */
1561 void transform_pivot_set_m4(float mat[4][4], const float pivot[3])
1562 {
1563         float tmat[4][4];
1564
1565         unit_m4(tmat);
1566
1567         copy_v3_v3(tmat[3], pivot);
1568         mul_m4_m4m4(mat, tmat, mat);
1569
1570         /* invert the matrix */
1571         negate_v3(tmat[3]);
1572         mul_m4_m4m4(mat, mat, tmat);
1573 }
1574
1575 void blend_m3_m3m3(float out[3][3], float dst[3][3], float src[3][3], const float srcweight)
1576 {
1577         float srot[3][3], drot[3][3];
1578         float squat[4], dquat[4], fquat[4];
1579         float sscale[3], dscale[3], fsize[3];
1580         float rmat[3][3], smat[3][3];
1581
1582         mat3_to_rot_size(drot, dscale, dst);
1583         mat3_to_rot_size(srot, sscale, src);
1584
1585         mat3_to_quat(dquat, drot);
1586         mat3_to_quat(squat, srot);
1587
1588         /* do blending */
1589         interp_qt_qtqt(fquat, dquat, squat, srcweight);
1590         interp_v3_v3v3(fsize, dscale, sscale, srcweight);
1591
1592         /* compose new matrix */
1593         quat_to_mat3(rmat, fquat);
1594         size_to_mat3(smat, fsize);
1595         mul_m3_m3m3(out, rmat, smat);
1596 }
1597
1598 void blend_m4_m4m4(float out[4][4], float dst[4][4], float src[4][4], const float srcweight)
1599 {
1600         float sloc[3], dloc[3], floc[3];
1601         float srot[3][3], drot[3][3];
1602         float squat[4], dquat[4], fquat[4];
1603         float sscale[3], dscale[3], fsize[3];
1604
1605         mat4_to_loc_rot_size(dloc, drot, dscale, dst);
1606         mat4_to_loc_rot_size(sloc, srot, sscale, src);
1607
1608         mat3_to_quat(dquat, drot);
1609         mat3_to_quat(squat, srot);
1610
1611         /* do blending */
1612         interp_v3_v3v3(floc, dloc, sloc, srcweight);
1613         interp_qt_qtqt(fquat, dquat, squat, srcweight);
1614         interp_v3_v3v3(fsize, dscale, sscale, srcweight);
1615
1616         /* compose new matrix */
1617         loc_quat_size_to_mat4(out, floc, fquat, fsize);
1618 }
1619
1620 bool is_negative_m3(float mat[3][3])
1621 {
1622         float vec[3];
1623         cross_v3_v3v3(vec, mat[0], mat[1]);
1624         return (dot_v3v3(vec, mat[2]) < 0.0f);
1625 }
1626
1627 bool is_negative_m4(float mat[4][4])
1628 {
1629         float vec[3];
1630         cross_v3_v3v3(vec, mat[0], mat[1]);
1631         return (dot_v3v3(vec, mat[2]) < 0.0f);
1632 }
1633
1634 bool is_zero_m3(float mat[3][3])
1635 {
1636         return (is_zero_v3(mat[0]) &&
1637                 is_zero_v3(mat[1]) &&
1638                 is_zero_v3(mat[2]));
1639 }
1640 bool is_zero_m4(float mat[4][4])
1641 {
1642         return (is_zero_v4(mat[0]) &&
1643                 is_zero_v4(mat[1]) &&
1644                 is_zero_v4(mat[2]) &&
1645                 is_zero_v4(mat[3]));
1646 }
1647
1648 /* make a 4x4 matrix out of 3 transform components */
1649 /* matrices are made in the order: scale * rot * loc */
1650 /* TODO: need to have a version that allows for rotation order... */
1651
1652 void loc_eul_size_to_mat4(float mat[4][4], const float loc[3], const float eul[3], const float size[3])
1653 {
1654         float rmat[3][3], smat[3][3], tmat[3][3];
1655
1656         /* initialize new matrix */
1657         unit_m4(mat);
1658
1659         /* make rotation + scaling part */
1660         eul_to_mat3(rmat, eul);
1661         size_to_mat3(smat, size);
1662         mul_m3_m3m3(tmat, rmat, smat);
1663
1664         /* copy rot/scale part to output matrix*/
1665         copy_m4_m3(mat, tmat);
1666
1667         /* copy location to matrix */
1668         mat[3][0] = loc[0];
1669         mat[3][1] = loc[1];
1670         mat[3][2] = loc[2];
1671 }
1672
1673 /* make a 4x4 matrix out of 3 transform components */
1674
1675 /* matrices are made in the order: scale * rot * loc */
1676 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)
1677 {
1678         float rmat[3][3], smat[3][3], tmat[3][3];
1679
1680         /* initialize new matrix */
1681         unit_m4(mat);
1682
1683         /* make rotation + scaling part */
1684         eulO_to_mat3(rmat, eul, rotOrder);
1685         size_to_mat3(smat, size);
1686         mul_m3_m3m3(tmat, rmat, smat);
1687
1688         /* copy rot/scale part to output matrix*/
1689         copy_m4_m3(mat, tmat);
1690
1691         /* copy location to matrix */
1692         mat[3][0] = loc[0];
1693         mat[3][1] = loc[1];
1694         mat[3][2] = loc[2];
1695 }
1696
1697
1698 /* make a 4x4 matrix out of 3 transform components */
1699
1700 /* matrices are made in the order: scale * rot * loc */
1701 void loc_quat_size_to_mat4(float mat[4][4], const float loc[3], const float quat[4], const float size[3])
1702 {
1703         float rmat[3][3], smat[3][3], tmat[3][3];
1704
1705         /* initialize new matrix */
1706         unit_m4(mat);
1707
1708         /* make rotation + scaling part */
1709         quat_to_mat3(rmat, quat);
1710         size_to_mat3(smat, size);
1711         mul_m3_m3m3(tmat, rmat, smat);
1712
1713         /* copy rot/scale part to output matrix*/
1714         copy_m4_m3(mat, tmat);
1715
1716         /* copy location to matrix */
1717         mat[3][0] = loc[0];
1718         mat[3][1] = loc[1];
1719         mat[3][2] = loc[2];
1720 }
1721
1722 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])
1723 {
1724         float q[4];
1725         axis_angle_to_quat(q, axis, angle);
1726         loc_quat_size_to_mat4(mat, loc, q, size);
1727 }
1728
1729 /*********************************** Other ***********************************/
1730
1731 void print_m3(const char *str, float m[3][3])
1732 {
1733         printf("%s\n", str);
1734         printf("%f %f %f\n", m[0][0], m[1][0], m[2][0]);
1735         printf("%f %f %f\n", m[0][1], m[1][1], m[2][1]);
1736         printf("%f %f %f\n", m[0][2], m[1][2], m[2][2]);
1737         printf("\n");
1738 }
1739
1740 void print_m4(const char *str, float m[4][4])
1741 {
1742         printf("%s\n", str);
1743         printf("%f %f %f %f\n", m[0][0], m[1][0], m[2][0], m[3][0]);
1744         printf("%f %f %f %f\n", m[0][1], m[1][1], m[2][1], m[3][1]);
1745         printf("%f %f %f %f\n", m[0][2], m[1][2], m[2][2], m[3][2]);
1746         printf("%f %f %f %f\n", m[0][3], m[1][3], m[2][3], m[3][3]);
1747         printf("\n");
1748 }
1749
1750 /*********************************** SVD ************************************
1751  * from TNT matrix library
1752  *
1753  * Compute the Single Value Decomposition of an arbitrary matrix A
1754  * That is compute the 3 matrices U,W,V with U column orthogonal (m,n)
1755  * ,W a diagonal matrix and V an orthogonal square matrix s.t.
1756  * A = U.W.Vt. From this decomposition it is trivial to compute the
1757  * (pseudo-inverse) of A as Ainv = V.Winv.tranpose(U).
1758  */
1759
1760 void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4])
1761 {
1762         float A[4][4];
1763         float work1[4], work2[4];
1764         int m = 4;
1765         int n = 4;
1766         int maxiter = 200;
1767         int nu = min_ii(m, n);
1768
1769         float *work = work1;
1770         float *e = work2;
1771         float eps;
1772
1773         int i = 0, j = 0, k = 0, p, pp, iter;
1774
1775         /* Reduce A to bidiagonal form, storing the diagonal elements
1776          * in s and the super-diagonal elements in e. */
1777
1778         int nct = min_ii(m - 1, n);
1779         int nrt = max_ii(0, min_ii(n - 2, m));
1780
1781         copy_m4_m4(A, A_);
1782         zero_m4(U);
1783         zero_v4(s);
1784
1785         for (k = 0; k < max_ii(nct, nrt); k++) {
1786                 if (k < nct) {
1787
1788                         /* Compute the transformation for the k-th column and
1789                          * place the k-th diagonal in s[k].
1790                          * Compute 2-norm of k-th column without under/overflow. */
1791                         s[k] = 0;
1792                         for (i = k; i < m; i++) {
1793                                 s[k] = hypotf(s[k], A[i][k]);
1794                         }
1795                         if (s[k] != 0.0f) {
1796                                 float invsk;
1797                                 if (A[k][k] < 0.0f) {
1798                                         s[k] = -s[k];
1799                                 }
1800                                 invsk = 1.0f / s[k];
1801                                 for (i = k; i < m; i++) {
1802                                         A[i][k] *= invsk;
1803                                 }
1804                                 A[k][k] += 1.0f;
1805                         }
1806                         s[k] = -s[k];
1807                 }
1808                 for (j = k + 1; j < n; j++) {
1809                         if ((k < nct) && (s[k] != 0.0f)) {
1810
1811                                 /* Apply the transformation. */
1812
1813                                 float t = 0;
1814                                 for (i = k; i < m; i++) {
1815                                         t += A[i][k] * A[i][j];
1816                                 }
1817                                 t = -t / A[k][k];
1818                                 for (i = k; i < m; i++) {
1819                                         A[i][j] += t * A[i][k];
1820                                 }
1821                         }
1822
1823                         /* Place the k-th row of A into e for the */
1824                         /* subsequent calculation of the row transformation. */
1825
1826                         e[j] = A[k][j];
1827                 }
1828                 if (k < nct) {
1829
1830                         /* Place the transformation in U for subsequent back
1831                          * multiplication. */
1832
1833                         for (i = k; i < m; i++)
1834                                 U[i][k] = A[i][k];
1835                 }
1836                 if (k < nrt) {
1837
1838                         /* Compute the k-th row transformation and place the
1839                          * k-th super-diagonal in e[k].
1840                          * Compute 2-norm without under/overflow. */
1841                         e[k] = 0;
1842                         for (i = k + 1; i < n; i++) {
1843                                 e[k] = hypotf(e[k], e[i]);
1844                         }
1845                         if (e[k] != 0.0f) {
1846                                 float invek;
1847                                 if (e[k + 1] < 0.0f) {
1848                                         e[k] = -e[k];
1849                                 }
1850                                 invek = 1.0f / e[k];
1851                                 for (i = k + 1; i < n; i++) {
1852                                         e[i] *= invek;
1853                                 }
1854                                 e[k + 1] += 1.0f;
1855                         }
1856                         e[k] = -e[k];
1857                         if ((k + 1 < m) & (e[k] != 0.0f)) {
1858                                 float invek1;
1859
1860                                 /* Apply the transformation. */
1861
1862                                 for (i = k + 1; i < m; i++) {
1863                                         work[i] = 0.0f;
1864                                 }
1865                                 for (j = k + 1; j < n; j++) {
1866                                         for (i = k + 1; i < m; i++) {
1867                                                 work[i] += e[j] * A[i][j];
1868                                         }
1869                                 }
1870                                 invek1 = 1.0f / e[k + 1];
1871                                 for (j = k + 1; j < n; j++) {
1872                                         float t = -e[j] * invek1;
1873                                         for (i = k + 1; i < m; i++) {
1874                                                 A[i][j] += t * work[i];
1875                                         }
1876                                 }
1877                         }
1878
1879                         /* Place the transformation in V for subsequent
1880                          * back multiplication. */
1881
1882                         for (i = k + 1; i < n; i++)
1883                                 V[i][k] = e[i];
1884                 }
1885         }
1886
1887         /* Set up the final bidiagonal matrix or order p. */
1888
1889         p = min_ii(n, m + 1);
1890         if (nct < n) {
1891                 s[nct] = A[nct][nct];
1892         }
1893         if (m < p) {
1894                 s[p - 1] = 0.0f;
1895         }
1896         if (nrt + 1 < p) {
1897                 e[nrt] = A[nrt][p - 1];
1898         }
1899         e[p - 1] = 0.0f;
1900
1901         /* If required, generate U. */
1902
1903         for (j = nct; j < nu; j++) {
1904                 for (i = 0; i < m; i++) {
1905                         U[i][j] = 0.0f;
1906                 }
1907                 U[j][j] = 1.0f;
1908         }
1909         for (k = nct - 1; k >= 0; k--) {
1910                 if (s[k] != 0.0f) {
1911                         for (j = k + 1; j < nu; j++) {
1912                                 float t = 0;
1913                                 for (i = k; i < m; i++) {
1914                                         t += U[i][k] * U[i][j];
1915                                 }
1916                                 t = -t / U[k][k];
1917                                 for (i = k; i < m; i++) {
1918                                         U[i][j] += t * U[i][k];
1919                                 }
1920                         }
1921                         for (i = k; i < m; i++) {
1922                                 U[i][k] = -U[i][k];
1923                         }
1924                         U[k][k] = 1.0f + U[k][k];
1925                         for (i = 0; i < k - 1; i++) {
1926                                 U[i][k] = 0.0f;
1927                         }
1928                 }
1929                 else {
1930                         for (i = 0; i < m; i++) {
1931                                 U[i][k] = 0.0f;
1932                         }
1933                         U[k][k] = 1.0f;
1934                 }
1935         }
1936
1937         /* If required, generate V. */
1938
1939         for (k = n - 1; k >= 0; k--) {
1940                 if ((k < nrt) & (e[k] != 0.0f)) {
1941                         for (j = k + 1; j < nu; j++) {
1942                                 float t = 0;
1943                                 for (i = k + 1; i < n; i++) {
1944                                         t += V[i][k] * V[i][j];
1945                                 }
1946                                 t = -t / V[k + 1][k];
1947                                 for (i = k + 1; i < n; i++) {
1948                                         V[i][j] += t * V[i][k];
1949                                 }
1950                         }
1951                 }
1952                 for (i = 0; i < n; i++) {
1953                         V[i][k] = 0.0f;
1954                 }
1955                 V[k][k] = 1.0f;
1956         }
1957
1958         /* Main iteration loop for the singular values. */
1959
1960         pp = p - 1;
1961         iter = 0;
1962         eps = powf(2.0f, -52.0f);
1963         while (p > 0) {
1964                 int kase = 0;
1965
1966                 /* Test for maximum iterations to avoid infinite loop */
1967                 if (maxiter == 0)
1968                         break;
1969                 maxiter--;
1970
1971                 /* This section of the program inspects for
1972                  * negligible elements in the s and e arrays.  On
1973                  * completion the variables kase and k are set as follows.
1974                  *
1975                  * kase = 1       if s(p) and e[k - 1] are negligible and k<p
1976                  * kase = 2       if s(k) is negligible and k<p
1977                  * kase = 3       if e[k - 1] is negligible, k<p, and
1978                  *               s(k), ..., s(p) are not negligible (qr step).
1979                  * kase = 4       if e(p - 1) is negligible (convergence). */
1980
1981                 for (k = p - 2; k >= -1; k--) {
1982                         if (k == -1) {
1983                                 break;
1984                         }
1985                         if (fabsf(e[k]) <= eps * (fabsf(s[k]) + fabsf(s[k + 1]))) {
1986                                 e[k] = 0.0f;
1987                                 break;
1988                         }
1989                 }
1990                 if (k == p - 2) {
1991                         kase = 4;
1992                 }
1993                 else {
1994                         int ks;
1995                         for (ks = p - 1; ks >= k; ks--) {
1996                                 float t;
1997                                 if (ks == k) {
1998                                         break;
1999                                 }
2000                                 t = (ks != p ? fabsf(e[ks]) : 0.f) +
2001                                     (ks != k + 1 ? fabsf(e[ks - 1]) : 0.0f);
2002                                 if (fabsf(s[ks]) <= eps * t) {
2003                                         s[ks] = 0.0f;
2004                                         break;
2005                                 }
2006                         }
2007                         if (ks == k) {
2008                                 kase = 3;
2009                         }
2010                         else if (ks == p - 1) {
2011                                 kase = 1;
2012                         }
2013                         else {
2014                                 kase = 2;
2015                                 k = ks;
2016                         }
2017                 }
2018                 k++;
2019
2020                 /* Perform the task indicated by kase. */
2021
2022                 switch (kase) {
2023
2024                         /* Deflate negligible s(p). */
2025
2026                         case 1:
2027                         {
2028                                 float f = e[p - 2];
2029                                 e[p - 2] = 0.0f;
2030                                 for (j = p - 2; j >= k; j--) {
2031                                         float t = hypotf(s[j], f);
2032                                         float invt = 1.0f / t;
2033                                         float cs = s[j] * invt;
2034                                         float sn = f * invt;
2035                                         s[j] = t;
2036                                         if (j != k) {
2037                                                 f = -sn * e[j - 1];
2038                                                 e[j - 1] = cs * e[j - 1];
2039                                         }
2040
2041                                         for (i = 0; i < n; i++) {
2042                                                 t = cs * V[i][j] + sn * V[i][p - 1];
2043                                                 V[i][p - 1] = -sn * V[i][j] + cs * V[i][p - 1];
2044                                                 V[i][j] = t;
2045                                         }
2046                                 }
2047                                 break;
2048                         }
2049
2050                         /* Split at negligible s(k). */
2051
2052                         case 2:
2053                         {
2054                                 float f = e[k - 1];
2055                                 e[k - 1] = 0.0f;
2056                                 for (j = k; j < p; j++) {
2057                                         float t = hypotf(s[j], f);
2058                                         float invt = 1.0f / t;
2059                                         float cs = s[j] * invt;
2060                                         float sn = f * invt;
2061                                         s[j] = t;
2062                                         f = -sn * e[j];
2063                                         e[j] = cs * e[j];
2064
2065                                         for (i = 0; i < m; i++) {
2066                                                 t = cs * U[i][j] + sn * U[i][k - 1];
2067                                                 U[i][k - 1] = -sn * U[i][j] + cs * U[i][k - 1];
2068                                                 U[i][j] = t;
2069                                         }
2070                                 }
2071                                 break;
2072                         }
2073
2074                         /* Perform one qr step. */
2075
2076                         case 3:
2077                         {
2078
2079                                 /* Calculate the shift. */
2080
2081                                 float scale = max_ff(max_ff(max_ff(max_ff(
2082                                                    fabsf(s[p - 1]), fabsf(s[p - 2])), fabsf(e[p - 2])),
2083                                                    fabsf(s[k])), fabsf(e[k]));
2084                                 float invscale = 1.0f / scale;
2085                                 float sp = s[p - 1] * invscale;
2086                                 float spm1 = s[p - 2] * invscale;
2087                                 float epm1 = e[p - 2] * invscale;
2088                                 float sk = s[k] * invscale;
2089                                 float ek = e[k] * invscale;
2090                                 float b = ((spm1 + sp) * (spm1 - sp) + epm1 * epm1) * 0.5f;
2091                                 float c = (sp * epm1) * (sp * epm1);
2092                                 float shift = 0.0f;
2093                                 float f, g;
2094                                 if ((b != 0.0f) || (c != 0.0f)) {
2095                                         shift = sqrtf(b * b + c);
2096                                         if (b < 0.0f) {
2097                                                 shift = -shift;
2098                                         }
2099                                         shift = c / (b + shift);
2100                                 }
2101                                 f = (sk + sp) * (sk - sp) + shift;
2102                                 g = sk * ek;
2103
2104                                 /* Chase zeros. */
2105
2106                                 for (j = k; j < p - 1; j++) {
2107                                         float t = hypotf(f, g);
2108                                         /* division by zero checks added to avoid NaN (brecht) */
2109                                         float cs = (t == 0.0f) ? 0.0f : f / t;
2110                                         float sn = (t == 0.0f) ? 0.0f : g / t;
2111                                         if (j != k) {
2112                                                 e[j - 1] = t;
2113                                         }
2114                                         f = cs * s[j] + sn * e[j];
2115                                         e[j] = cs * e[j] - sn * s[j];
2116                                         g = sn * s[j + 1];
2117                                         s[j + 1] = cs * s[j + 1];
2118
2119                                         for (i = 0; i < n; i++) {
2120                                                 t = cs * V[i][j] + sn * V[i][j + 1];
2121                                                 V[i][j + 1] = -sn * V[i][j] + cs * V[i][j + 1];
2122                                                 V[i][j] = t;
2123                                         }
2124
2125                                         t = hypotf(f, g);
2126                                         /* division by zero checks added to avoid NaN (brecht) */
2127                                         cs = (t == 0.0f) ? 0.0f : f / t;
2128                                         sn = (t == 0.0f) ? 0.0f : g / t;
2129                                         s[j] = t;
2130                                         f = cs * e[j] + sn * s[j + 1];
2131                                         s[j + 1] = -sn * e[j] + cs * s[j + 1];
2132                                         g = sn * e[j + 1];
2133                                         e[j + 1] = cs * e[j + 1];
2134                                         if (j < m - 1) {
2135                                                 for (i = 0; i < m; i++) {
2136                                                         t = cs * U[i][j] + sn * U[i][j + 1];
2137                                                         U[i][j + 1] = -sn * U[i][j] + cs * U[i][j + 1];
2138                                                         U[i][j] = t;
2139                                                 }
2140                                         }
2141                                 }
2142                                 e[p - 2] = f;
2143                                 iter = iter + 1;
2144                                 break;
2145                         }
2146                         /* Convergence. */
2147
2148                         case 4:
2149                         {
2150
2151                                 /* Make the singular values positive. */
2152
2153                                 if (s[k] <= 0.0f) {
2154                                         s[k] = (s[k] < 0.0f ? -s[k] : 0.0f);
2155
2156                                         for (i = 0; i <= pp; i++)
2157                                                 V[i][k] = -V[i][k];
2158                                 }
2159
2160                                 /* Order the singular values. */
2161
2162                                 while (k < pp) {
2163                                         float t;
2164                                         if (s[k] >= s[k + 1]) {
2165                                                 break;
2166                                         }
2167                                         t = s[k];
2168                                         s[k] = s[k + 1];
2169                                         s[k + 1] = t;
2170                                         if (k < n - 1) {
2171                                                 for (i = 0; i < n; i++) {
2172                                                         t = V[i][k + 1];
2173                                                         V[i][k + 1] = V[i][k];
2174                                                         V[i][k] = t;
2175                                                 }
2176                                         }
2177                                         if (k < m - 1) {
2178                                                 for (i = 0; i < m; i++) {
2179                                                         t = U[i][k + 1];
2180                                                         U[i][k + 1] = U[i][k];
2181                                                         U[i][k] = t;
2182                                                 }
2183                                         }
2184                                         k++;
2185                                 }
2186                                 iter = 0;
2187                                 p--;
2188                                 break;
2189                         }
2190                 }
2191         }
2192 }
2193
2194 void pseudoinverse_m4_m4(float Ainv[4][4], float A[4][4], float epsilon)
2195 {
2196         /* compute moon-penrose pseudo inverse of matrix, singular values
2197          * below epsilon are ignored for stability (truncated SVD) */
2198         float V[4][4], W[4], Wm[4][4], U[4][4];
2199         int i;
2200
2201         transpose_m4(A);
2202         svd_m4(V, W, U, A);
2203         transpose_m4(U);
2204         transpose_m4(V);
2205
2206         zero_m4(Wm);
2207         for (i = 0; i < 4; i++)
2208                 Wm[i][i] = (W[i] < epsilon) ? 0.0f : 1.0f / W[i];
2209
2210         transpose_m4(V);
2211
2212         mul_m4_series(Ainv, U, Wm, V);
2213 }
2214
2215 void pseudoinverse_m3_m3(float Ainv[3][3], float A[3][3], float epsilon)
2216 {
2217         /* try regular inverse when possible, otherwise fall back to slow svd */
2218         if (!invert_m3_m3(Ainv, A)) {
2219                 float tmp[4][4], tmpinv[4][4];
2220
2221                 copy_m4_m3(tmp, A);
2222                 pseudoinverse_m4_m4(tmpinv, tmp, epsilon);
2223                 copy_m3_m4(Ainv, tmpinv);
2224         }
2225 }
2226
2227 bool has_zero_axis_m4(float matrix[4][4])
2228 {
2229         return len_squared_v3(matrix[0]) < FLT_EPSILON ||
2230                len_squared_v3(matrix[1]) < FLT_EPSILON ||
2231                len_squared_v3(matrix[2]) < FLT_EPSILON;
2232 }
2233
2234 void invert_m4_m4_safe(float Ainv[4][4], float A[4][4])
2235 {
2236         if (!invert_m4_m4(Ainv, A)) {
2237                 float Atemp[4][4];
2238
2239                 copy_m4_m4(Atemp, A);
2240
2241                 /* Matrix is degenerate (e.g. 0 scale on some axis), ideally we should
2242                  * never be in this situation, but try to invert it anyway with tweak.
2243                  */
2244                 Atemp[0][0] += 1e-8f;
2245                 Atemp[1][1] += 1e-8f;
2246                 Atemp[2][2] += 1e-8f;
2247
2248                 if (!invert_m4_m4(Ainv, Atemp)) {
2249                         unit_m4(Ainv);
2250                 }
2251         }
2252 }
2253
2254 /**
2255  * SpaceTransform struct encapsulates all needed data to convert between two coordinate spaces
2256  * (where conversion can be represented by a matrix multiplication).
2257  *
2258  * A SpaceTransform is initialized using:
2259  *   BLI_SPACE_TRANSFORM_SETUP(&data,  ob1, ob2)
2260  *
2261  * After that the following calls can be used:
2262  *   BLI_space_transform_apply(&data, co);  // converts a coordinate in ob1 space to the corresponding ob2 space
2263  *   BLI_space_transform_invert(&data, co);  // converts a coordinate in ob2 space to the corresponding ob1 space
2264  *
2265  * Same concept as BLI_space_transform_apply and BLI_space_transform_invert, but no is normalized after conversion
2266  * (and not translated at all!):
2267  *   BLI_space_transform_apply_normal(&data, no);
2268  *   BLI_space_transform_invert_normal(&data, no);
2269  *
2270  */
2271
2272 void BLI_space_transform_from_matrices(SpaceTransform *data, float local[4][4], float target[4][4])
2273 {
2274         float itarget[4][4];
2275         invert_m4_m4(itarget, target);
2276         mul_m4_m4m4(data->local2target, itarget, local);
2277         invert_m4_m4(data->target2local, data->local2target);
2278 }
2279
2280 void BLI_space_transform_apply(const SpaceTransform *data, float co[3])
2281 {
2282         mul_v3_m4v3(co, ((SpaceTransform *)data)->local2target, co);
2283 }
2284
2285 void BLI_space_transform_invert(const SpaceTransform *data, float co[3])
2286 {
2287         mul_v3_m4v3(co, ((SpaceTransform *)data)->target2local, co);
2288 }
2289
2290 void BLI_space_transform_apply_normal(const SpaceTransform *data, float no[3])
2291 {
2292         mul_mat3_m4_v3(((SpaceTransform *)data)->local2target, no);
2293         normalize_v3(no);
2294 }
2295
2296 void BLI_space_transform_invert_normal(const SpaceTransform *data, float no[3])
2297 {
2298         mul_mat3_m4_v3(((SpaceTransform *)data)->target2local, no);
2299         normalize_v3(no);
2300 }