26576b2dcb292e2da182bb6816fd1683908cc83b
[blender-staging.git] / source / blender / blenlib / intern / math_rotation.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_rotation.c
27  *  \ingroup bli
28  */
29
30
31
32 #include <assert.h>
33 #include "BLI_math.h"
34
35 /******************************** Quaternions ********************************/
36
37 /* used to test is a quat is not normalized (only used for debug prints) */
38 #ifdef DEBUG
39 #  define QUAT_EPSILON 0.0001
40 #endif
41
42 /* convenience, avoids setting Y axis everywhere */
43 void unit_axis_angle(float axis[3], float *angle)
44 {
45         axis[0] = 0.0f;
46         axis[1] = 1.0f;
47         axis[2] = 0.0f;
48         *angle = 0.0f;
49 }
50
51 void unit_qt(float q[4])
52 {
53         q[0] = 1.0f;
54         q[1] = q[2] = q[3] = 0.0f;
55 }
56
57 void copy_qt_qt(float q1[4], const float q2[4])
58 {
59         q1[0] = q2[0];
60         q1[1] = q2[1];
61         q1[2] = q2[2];
62         q1[3] = q2[3];
63 }
64
65 int is_zero_qt(float *q)
66 {
67         return (q[0] == 0 && q[1] == 0 && q[2] == 0 && q[3] == 0);
68 }
69
70 void mul_qt_qtqt(float q[4], const float q1[4], const float q2[4])
71 {
72         float t0, t1, t2;
73
74         t0 = q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3];
75         t1 = q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2];
76         t2 = q1[0] * q2[2] + q1[2] * q2[0] + q1[3] * q2[1] - q1[1] * q2[3];
77         q[3] = q1[0] * q2[3] + q1[3] * q2[0] + q1[1] * q2[2] - q1[2] * q2[1];
78         q[0] = t0;
79         q[1] = t1;
80         q[2] = t2;
81 }
82
83 /**
84  * \note:
85  * Assumes a unit quaternion?
86  *
87  * \note: multiplying by 3x3 matrix is ~25% faster.
88  *
89  * in fact not, but you may want to use a unit quat, read on...
90  *
91  * Shortcut for 'q v q*' when \a v is actually a quaternion.
92  * This removes the need for converting a vector to a quaternion,
93  * calculating q's conjugate and converting back to a vector.
94  * It also happens to be faster (17+,24* vs * 24+,32*).
95  * If \a q is not a unit quaternion, then \a v will be both rotated by
96  * the same amount as if q was a unit quaternion, and scaled by the square of
97  * the length of q.
98  *
99  * For people used to python mathutils, its like:
100  * def mul_qt_v3(q, v): (q * Quaternion((0.0, v[0], v[1], v[2])) * q.conjugated())[1:]
101  */
102 void mul_qt_v3(const float q[4], float v[3])
103 {
104         float t0, t1, t2;
105
106         t0 = -q[1] * v[0] - q[2] * v[1] - q[3] * v[2];
107         t1 = q[0] * v[0] + q[2] * v[2] - q[3] * v[1];
108         t2 = q[0] * v[1] + q[3] * v[0] - q[1] * v[2];
109         v[2] = q[0] * v[2] + q[1] * v[1] - q[2] * v[0];
110         v[0] = t1;
111         v[1] = t2;
112
113         t1 = t0 * -q[1] + v[0] * q[0] - v[1] * q[3] + v[2] * q[2];
114         t2 = t0 * -q[2] + v[1] * q[0] - v[2] * q[1] + v[0] * q[3];
115         v[2] = t0 * -q[3] + v[2] * q[0] - v[0] * q[2] + v[1] * q[1];
116         v[0] = t1;
117         v[1] = t2;
118 }
119
120 void conjugate_qt_qt(float q1[4], const float q2[4])
121 {
122         q1[0] =  q2[0];
123         q1[1] = -q2[1];
124         q1[2] = -q2[2];
125         q1[3] = -q2[3];
126 }
127
128 void conjugate_qt(float q[4])
129 {
130         q[1] = -q[1];
131         q[2] = -q[2];
132         q[3] = -q[3];
133 }
134
135 float dot_qtqt(const float q1[4], const float q2[4])
136 {
137         return q1[0] * q2[0] + q1[1] * q2[1] + q1[2] * q2[2] + q1[3] * q2[3];
138 }
139
140 void invert_qt(float q[4])
141 {
142         float f = dot_qtqt(q, q);
143
144         if (f == 0.0f)
145                 return;
146
147         conjugate_qt(q);
148         mul_qt_fl(q, 1.0f / f);
149 }
150
151 void invert_qt_qt(float q1[4], const float q2[4])
152 {
153         copy_qt_qt(q1, q2);
154         invert_qt(q1);
155 }
156
157 /* simple mult */
158 void mul_qt_fl(float q[4], const float f)
159 {
160         q[0] *= f;
161         q[1] *= f;
162         q[2] *= f;
163         q[3] *= f;
164 }
165
166 void sub_qt_qtqt(float q[4], const float q1[4], const float q2[4])
167 {
168         float nq2[4];
169
170         nq2[0] = -q2[0];
171         nq2[1] = q2[1];
172         nq2[2] = q2[2];
173         nq2[3] = q2[3];
174
175         mul_qt_qtqt(q, q1, nq2);
176 }
177
178 /* angular mult factor */
179 void mul_fac_qt_fl(float q[4], const float fac)
180 {
181         const float angle = fac * saacos(q[0]); /* quat[0] = cos(0.5 * angle), but now the 0.5 and 2.0 rule out */
182         const float co = cosf(angle);
183         const float si = sinf(angle);
184         q[0] = co;
185         normalize_v3(q + 1);
186         mul_v3_fl(q + 1, si);
187 }
188
189 /* skip error check, currently only needed by mat3_to_quat_is_ok */
190 static void quat_to_mat3_no_error(float m[3][3], const float q[4])
191 {
192         double q0, q1, q2, q3, qda, qdb, qdc, qaa, qab, qac, qbb, qbc, qcc;
193
194         q0 = M_SQRT2 * (double)q[0];
195         q1 = M_SQRT2 * (double)q[1];
196         q2 = M_SQRT2 * (double)q[2];
197         q3 = M_SQRT2 * (double)q[3];
198
199         qda = q0 * q1;
200         qdb = q0 * q2;
201         qdc = q0 * q3;
202         qaa = q1 * q1;
203         qab = q1 * q2;
204         qac = q1 * q3;
205         qbb = q2 * q2;
206         qbc = q2 * q3;
207         qcc = q3 * q3;
208
209         m[0][0] = (float)(1.0 - qbb - qcc);
210         m[0][1] = (float)(qdc + qab);
211         m[0][2] = (float)(-qdb + qac);
212
213         m[1][0] = (float)(-qdc + qab);
214         m[1][1] = (float)(1.0 - qaa - qcc);
215         m[1][2] = (float)(qda + qbc);
216
217         m[2][0] = (float)(qdb + qac);
218         m[2][1] = (float)(-qda + qbc);
219         m[2][2] = (float)(1.0 - qaa - qbb);
220 }
221
222 void quat_to_mat3(float m[3][3], const float q[4])
223 {
224 #ifdef DEBUG
225         float f;
226         if (!((f = dot_qtqt(q, q)) == 0.0f || (fabsf(f - 1.0f) < (float)QUAT_EPSILON))) {
227                 fprintf(stderr, "Warning! quat_to_mat3() called with non-normalized: size %.8f *** report a bug ***\n", f);
228         }
229 #endif
230
231         quat_to_mat3_no_error(m, q);
232 }
233
234 void quat_to_mat4(float m[4][4], const float q[4])
235 {
236         double q0, q1, q2, q3, qda, qdb, qdc, qaa, qab, qac, qbb, qbc, qcc;
237
238 #ifdef DEBUG
239         if (!((q0 = dot_qtqt(q, q)) == 0.0 || (fabs(q0 - 1.0) < QUAT_EPSILON))) {
240                 fprintf(stderr, "Warning! quat_to_mat4() called with non-normalized: size %.8f *** report a bug ***\n", (float)q0);
241         }
242 #endif
243
244         q0 = M_SQRT2 * (double)q[0];
245         q1 = M_SQRT2 * (double)q[1];
246         q2 = M_SQRT2 * (double)q[2];
247         q3 = M_SQRT2 * (double)q[3];
248
249         qda = q0 * q1;
250         qdb = q0 * q2;
251         qdc = q0 * q3;
252         qaa = q1 * q1;
253         qab = q1 * q2;
254         qac = q1 * q3;
255         qbb = q2 * q2;
256         qbc = q2 * q3;
257         qcc = q3 * q3;
258
259         m[0][0] = (float)(1.0 - qbb - qcc);
260         m[0][1] = (float)(qdc + qab);
261         m[0][2] = (float)(-qdb + qac);
262         m[0][3] = 0.0f;
263
264         m[1][0] = (float)(-qdc + qab);
265         m[1][1] = (float)(1.0 - qaa - qcc);
266         m[1][2] = (float)(qda + qbc);
267         m[1][3] = 0.0f;
268
269         m[2][0] = (float)(qdb + qac);
270         m[2][1] = (float)(-qda + qbc);
271         m[2][2] = (float)(1.0 - qaa - qbb);
272         m[2][3] = 0.0f;
273
274         m[3][0] = m[3][1] = m[3][2] = 0.0f;
275         m[3][3] = 1.0f;
276 }
277
278 void mat3_to_quat(float q[4], float wmat[3][3])
279 {
280         double tr, s;
281         float mat[3][3];
282
283         /* work on a copy */
284         copy_m3_m3(mat, wmat);
285         normalize_m3(mat); /* this is needed AND a 'normalize_qt' in the end */
286
287         tr = 0.25 * (double)(1.0f + mat[0][0] + mat[1][1] + mat[2][2]);
288
289         if (tr > (double)FLT_EPSILON) {
290                 s = sqrt(tr);
291                 q[0] = (float)s;
292                 s = 1.0 / (4.0 * s);
293                 q[1] = (float)((double)(mat[1][2] - mat[2][1]) * s);
294                 q[2] = (float)((double)(mat[2][0] - mat[0][2]) * s);
295                 q[3] = (float)((double)(mat[0][1] - mat[1][0]) * s);
296         }
297         else {
298                 if (mat[0][0] > mat[1][1] && mat[0][0] > mat[2][2]) {
299                         s = 2.0f * sqrtf(1.0f + mat[0][0] - mat[1][1] - mat[2][2]);
300                         q[1] = (float)(0.25 * s);
301
302                         s = 1.0 / s;
303                         q[0] = (float)((double)(mat[2][1] - mat[1][2]) * s);
304                         q[2] = (float)((double)(mat[1][0] + mat[0][1]) * s);
305                         q[3] = (float)((double)(mat[2][0] + mat[0][2]) * s);
306                 }
307                 else if (mat[1][1] > mat[2][2]) {
308                         s = 2.0f * sqrtf(1.0f + mat[1][1] - mat[0][0] - mat[2][2]);
309                         q[2] = (float)(0.25 * s);
310
311                         s = 1.0 / s;
312                         q[0] = (float)((double)(mat[2][0] - mat[0][2]) * s);
313                         q[1] = (float)((double)(mat[1][0] + mat[0][1]) * s);
314                         q[3] = (float)((double)(mat[2][1] + mat[1][2]) * s);
315                 }
316                 else {
317                         s = 2.0f * sqrtf(1.0f + mat[2][2] - mat[0][0] - mat[1][1]);
318                         q[3] = (float)(0.25 * s);
319
320                         s = 1.0 / s;
321                         q[0] = (float)((double)(mat[1][0] - mat[0][1]) * s);
322                         q[1] = (float)((double)(mat[2][0] + mat[0][2]) * s);
323                         q[2] = (float)((double)(mat[2][1] + mat[1][2]) * s);
324                 }
325         }
326
327         normalize_qt(q);
328 }
329
330 void mat4_to_quat(float q[4], float m[4][4])
331 {
332         float mat[3][3];
333
334         copy_m3_m4(mat, m);
335         mat3_to_quat(q, mat);
336 }
337
338 void mat3_to_quat_is_ok(float q[4], float wmat[3][3])
339 {
340         float mat[3][3], matr[3][3], matn[3][3], q1[4], q2[4], angle, si, co, nor[3];
341
342         /* work on a copy */
343         copy_m3_m3(mat, wmat);
344         normalize_m3(mat);
345
346         /* rotate z-axis of matrix to z-axis */
347
348         nor[0] = mat[2][1]; /* cross product with (0,0,1) */
349         nor[1] = -mat[2][0];
350         nor[2] = 0.0;
351         normalize_v3(nor);
352
353         co = mat[2][2];
354         angle = 0.5f * saacos(co);
355
356         co = cosf(angle);
357         si = sinf(angle);
358         q1[0] = co;
359         q1[1] = -nor[0] * si; /* negative here, but why? */
360         q1[2] = -nor[1] * si;
361         q1[3] = -nor[2] * si;
362
363         /* rotate back x-axis from mat, using inverse q1 */
364         quat_to_mat3_no_error(matr, q1);
365         invert_m3_m3(matn, matr);
366         mul_m3_v3(matn, mat[0]);
367
368         /* and align x-axes */
369         angle = (float)(0.5 * atan2(mat[0][1], mat[0][0]));
370
371         co = cosf(angle);
372         si = sinf(angle);
373         q2[0] = co;
374         q2[1] = 0.0f;
375         q2[2] = 0.0f;
376         q2[3] = si;
377
378         mul_qt_qtqt(q, q1, q2);
379 }
380
381 float normalize_qt(float q[4])
382 {
383         float len;
384
385         len = sqrtf(dot_qtqt(q, q));
386         if (len != 0.0f) {
387                 mul_qt_fl(q, 1.0f / len);
388         }
389         else {
390                 q[1] = 1.0f;
391                 q[0] = q[2] = q[3] = 0.0f;
392         }
393
394         return len;
395 }
396
397 float normalize_qt_qt(float r[4], const float q[4])
398 {
399         copy_qt_qt(r, q);
400         return normalize_qt(r);
401 }
402
403 /* note: expects vectors to be normalized */
404 void rotation_between_vecs_to_quat(float q[4], const float v1[3], const float v2[3])
405 {
406         float axis[3];
407         float angle;
408
409         cross_v3_v3v3(axis, v1, v2);
410
411         angle = angle_normalized_v3v3(v1, v2);
412
413         axis_angle_to_quat(q, axis, angle);
414 }
415
416 void rotation_between_quats_to_quat(float q[4], const float q1[4], const float q2[4])
417 {
418         float tquat[4];
419
420         conjugate_qt_qt(tquat, q1);
421
422         mul_qt_fl(tquat, 1.0f / dot_qtqt(tquat, tquat));
423
424         mul_qt_qtqt(q, tquat, q2);
425 }
426
427 void vec_to_quat(float q[4], const float vec[3], short axis, const short upflag)
428 {
429         float nor[3], tvec[3];
430         float angle, si, co, len;
431
432         assert(axis >= 0 && axis <= 5);
433         assert(upflag >= 0 && upflag <= 2);
434
435         /* first set the quat to unit */
436         unit_qt(q);
437
438         len = len_v3(vec);
439
440         if (UNLIKELY(len == 0.0f)) {
441                 return;
442         }
443
444         /* rotate to axis */
445         if (axis > 2) {
446                 copy_v3_v3(tvec, vec);
447                 axis -= 3;
448         }
449         else {
450                 negate_v3_v3(tvec, vec);
451         }
452
453         /* nasty! I need a good routine for this...
454          * problem is a rotation of an Y axis to the negative Y-axis for example.
455          */
456
457         if (axis == 0) { /* x-axis */
458                 nor[0] =  0.0;
459                 nor[1] = -tvec[2];
460                 nor[2] =  tvec[1];
461
462                 if (fabsf(tvec[1]) + fabsf(tvec[2]) < 0.0001f)
463                         nor[1] = 1.0f;
464
465                 co = tvec[0];
466         }
467         else if (axis == 1) { /* y-axis */
468                 nor[0] =  tvec[2];
469                 nor[1] =  0.0;
470                 nor[2] = -tvec[0];
471
472                 if (fabsf(tvec[0]) + fabsf(tvec[2]) < 0.0001f)
473                         nor[2] = 1.0f;
474
475                 co = tvec[1];
476         }
477         else { /* z-axis */
478                 nor[0] = -tvec[1];
479                 nor[1] =  tvec[0];
480                 nor[2] =  0.0;
481
482                 if (fabsf(tvec[0]) + fabsf(tvec[1]) < 0.0001f)
483                         nor[0] = 1.0f;
484
485                 co = tvec[2];
486         }
487         co /= len;
488
489         normalize_v3(nor);
490
491         angle = 0.5f * saacos(co);
492         si   = sinf(angle);
493         q[0] = cosf(angle);
494         q[1] = nor[0] * si;
495         q[2] = nor[1] * si;
496         q[3] = nor[2] * si;
497
498         if (axis != upflag) {
499                 float mat[3][3];
500                 float q2[4];
501                 const float *fp = mat[2];
502                 quat_to_mat3(mat, q);
503
504                 if (axis == 0) {
505                         if (upflag == 1) angle =  0.5f * atan2f(fp[2], fp[1]);
506                         else             angle = -0.5f * atan2f(fp[1], fp[2]);
507                 }
508                 else if (axis == 1) {
509                         if (upflag == 0) angle = -0.5f * atan2f(fp[2], fp[0]);
510                         else             angle =  0.5f * atan2f(fp[0], fp[2]);
511                 }
512                 else {
513                         if (upflag == 0) angle =  0.5f * atan2f(-fp[1], -fp[0]);
514                         else             angle = -0.5f * atan2f(-fp[0], -fp[1]);
515                 }
516
517                 co = cosf(angle);
518                 si = sinf(angle) / len;
519                 q2[0] = co;
520                 q2[1] = tvec[0] * si;
521                 q2[2] = tvec[1] * si;
522                 q2[3] = tvec[2] * si;
523
524                 mul_qt_qtqt(q, q2, q);
525         }
526 }
527
528 #if 0
529
530 /* A & M Watt, Advanced animation and rendering techniques, 1992 ACM press */
531 void QuatInterpolW(float *result, float quat1[4], float quat2[4], float t)
532 {
533         float omega, cosom, sinom, sc1, sc2;
534
535         cosom = quat1[0] * quat2[0] + quat1[1] * quat2[1] + quat1[2] * quat2[2] + quat1[3] * quat2[3];
536
537         /* rotate around shortest angle */
538         if ((1.0f + cosom) > 0.0001f) {
539
540                 if ((1.0f - cosom) > 0.0001f) {
541                         omega = (float)acos(cosom);
542                         sinom = (float)sin(omega);
543                         sc1 = (float)sin((1.0 - t) * omega) / sinom;
544                         sc2 = (float)sin(t * omega) / sinom;
545                 }
546                 else {
547                         sc1 = 1.0f - t;
548                         sc2 = t;
549                 }
550                 result[0] = sc1 * quat1[0] + sc2 * quat2[0];
551                 result[1] = sc1 * quat1[1] + sc2 * quat2[1];
552                 result[2] = sc1 * quat1[2] + sc2 * quat2[2];
553                 result[3] = sc1 * quat1[3] + sc2 * quat2[3];
554         }
555         else {
556                 result[0] = quat2[3];
557                 result[1] = -quat2[2];
558                 result[2] = quat2[1];
559                 result[3] = -quat2[0];
560
561                 sc1 = (float)sin((1.0 - t) * M_PI_2);
562                 sc2 = (float)sin(t * M_PI_2);
563
564                 result[0] = sc1 * quat1[0] + sc2 * result[0];
565                 result[1] = sc1 * quat1[1] + sc2 * result[1];
566                 result[2] = sc1 * quat1[2] + sc2 * result[2];
567                 result[3] = sc1 * quat1[3] + sc2 * result[3];
568         }
569 }
570 #endif
571
572 void interp_qt_qtqt(float result[4], const float quat1[4], const float quat2[4], const float t)
573 {
574         float quat[4], omega, cosom, sinom, sc1, sc2;
575
576         cosom = quat1[0] * quat2[0] + quat1[1] * quat2[1] + quat1[2] * quat2[2] + quat1[3] * quat2[3];
577
578         /* rotate around shortest angle */
579         if (cosom < 0.0f) {
580                 cosom = -cosom;
581                 quat[0] = -quat1[0];
582                 quat[1] = -quat1[1];
583                 quat[2] = -quat1[2];
584                 quat[3] = -quat1[3];
585         }
586         else {
587                 quat[0] = quat1[0];
588                 quat[1] = quat1[1];
589                 quat[2] = quat1[2];
590                 quat[3] = quat1[3];
591         }
592
593         if ((1.0f - cosom) > 0.0001f) {
594                 omega = acosf(cosom);
595                 sinom = sinf(omega);
596                 sc1 = sinf((1.0f - t) * omega) / sinom;
597                 sc2 = sinf(t * omega) / sinom;
598         }
599         else {
600                 sc1 = 1.0f - t;
601                 sc2 = t;
602         }
603
604         result[0] = sc1 * quat[0] + sc2 * quat2[0];
605         result[1] = sc1 * quat[1] + sc2 * quat2[1];
606         result[2] = sc1 * quat[2] + sc2 * quat2[2];
607         result[3] = sc1 * quat[3] + sc2 * quat2[3];
608 }
609
610 void add_qt_qtqt(float result[4], const float quat1[4], const float quat2[4], const float t)
611 {
612         result[0] = quat1[0] + t * quat2[0];
613         result[1] = quat1[1] + t * quat2[1];
614         result[2] = quat1[2] + t * quat2[2];
615         result[3] = quat1[3] + t * quat2[3];
616 }
617
618 /* same as tri_to_quat() but takes pre-computed normal from the triangle
619  * used for ngons when we know their normal */
620 void tri_to_quat_ex(float quat[4], const float v1[3], const float v2[3], const float v3[3],
621                     const float no_orig[3])
622 {
623         /* imaginary x-axis, y-axis triangle is being rotated */
624         float vec[3], q1[4], q2[4], n[3], si, co, angle, mat[3][3], imat[3][3];
625
626         /* move z-axis to face-normal */
627 #if 0
628         normal_tri_v3(vec, v1, v2, v3);
629 #else
630         copy_v3_v3(vec, no_orig);
631         (void)v3;
632 #endif
633
634         n[0] =  vec[1];
635         n[1] = -vec[0];
636         n[2] =  0.0f;
637         normalize_v3(n);
638
639         if (n[0] == 0.0f && n[1] == 0.0f) {
640                 n[0] = 1.0f;
641         }
642
643         angle = -0.5f * (float)saacos(vec[2]);
644         co = cosf(angle);
645         si = sinf(angle);
646         q1[0] = co;
647         q1[1] = n[0] * si;
648         q1[2] = n[1] * si;
649         q1[3] = 0.0f;
650
651         /* rotate back line v1-v2 */
652         quat_to_mat3(mat, q1);
653         invert_m3_m3(imat, mat);
654         sub_v3_v3v3(vec, v2, v1);
655         mul_m3_v3(imat, vec);
656
657         /* what angle has this line with x-axis? */
658         vec[2] = 0.0f;
659         normalize_v3(vec);
660
661         angle = (float)(0.5 * atan2(vec[1], vec[0]));
662         co = cosf(angle);
663         si = sinf(angle);
664         q2[0] = co;
665         q2[1] = 0.0f;
666         q2[2] = 0.0f;
667         q2[3] = si;
668
669         mul_qt_qtqt(quat, q1, q2);
670 }
671
672 void tri_to_quat(float quat[4], const float v1[3], const float v2[3], const float v3[3])
673 {
674         float vec[3];
675         normal_tri_v3(vec, v1, v2, v3);
676         tri_to_quat_ex(quat, v1, v2, v3, vec);
677 }
678
679 void print_qt(const char *str, const float q[4])
680 {
681         printf("%s: %.3f %.3f %.3f %.3f\n", str, q[0], q[1], q[2], q[3]);
682 }
683
684 /******************************** Axis Angle *********************************/
685
686 /* Axis angle to Quaternions */
687 void axis_angle_to_quat(float q[4], const float axis[3], const float angle)
688 {
689         float nor[3];
690
691         if (LIKELY(normalize_v3_v3(nor, axis) != 0.0f)) {
692                 const float phi = angle / 2.0f;
693                 float si;
694                 si   = sinf(phi);
695                 q[0] = cosf(phi);
696                 q[1] = nor[0] * si;
697                 q[2] = nor[1] * si;
698                 q[3] = nor[2] * si;
699         }
700         else {
701                 unit_qt(q);
702         }
703 }
704
705 /* Quaternions to Axis Angle */
706 void quat_to_axis_angle(float axis[3], float *angle, const float q[4])
707 {
708         float ha, si;
709
710 #ifdef DEBUG
711         if (!((ha = dot_qtqt(q, q)) == 0.0f || (fabsf(ha - 1.0f) < (float)QUAT_EPSILON))) {
712                 fprintf(stderr, "Warning! quat_to_axis_angle() called with non-normalized: size %.8f *** report a bug ***\n", ha);
713         }
714 #endif
715
716         /* calculate angle/2, and sin(angle/2) */
717         ha = acosf(q[0]);
718         si = sinf(ha);
719
720         /* from half-angle to angle */
721         *angle = ha * 2;
722
723         /* prevent division by zero for axis conversion */
724         if (fabsf(si) < 0.0005f)
725                 si = 1.0f;
726
727         axis[0] = q[1] / si;
728         axis[1] = q[2] / si;
729         axis[2] = q[3] / si;
730 }
731
732 /* Axis Angle to Euler Rotation */
733 void axis_angle_to_eulO(float eul[3], const short order, const float axis[3], const float angle)
734 {
735         float q[4];
736
737         /* use quaternions as intermediate representation for now... */
738         axis_angle_to_quat(q, axis, angle);
739         quat_to_eulO(eul, order, q);
740 }
741
742 /* Euler Rotation to Axis Angle */
743 void eulO_to_axis_angle(float axis[3], float *angle, const float eul[3], const short order)
744 {
745         float q[4];
746
747         /* use quaternions as intermediate representation for now... */
748         eulO_to_quat(q, eul, order);
749         quat_to_axis_angle(axis, angle, q);
750 }
751
752 /* axis angle to 3x3 matrix - safer version (normalization of axis performed)
753  *
754  * note: we may want a normalized and non normalized version of this function.
755  */
756 void axis_angle_to_mat3(float mat[3][3], const float axis[3], const float angle)
757 {
758         float nor[3], nsi[3], co, si, ico;
759
760         /* normalize the axis first (to remove unwanted scaling) */
761         if (normalize_v3_v3(nor, axis) == 0.0f) {
762                 unit_m3(mat);
763                 return;
764         }
765
766         /* now convert this to a 3x3 matrix */
767         co = cosf(angle);
768         si = sinf(angle);
769
770         ico = (1.0f - co);
771         nsi[0] = nor[0] * si;
772         nsi[1] = nor[1] * si;
773         nsi[2] = nor[2] * si;
774
775         mat[0][0] = ((nor[0] * nor[0]) * ico) + co;
776         mat[0][1] = ((nor[0] * nor[1]) * ico) + nsi[2];
777         mat[0][2] = ((nor[0] * nor[2]) * ico) - nsi[1];
778         mat[1][0] = ((nor[0] * nor[1]) * ico) - nsi[2];
779         mat[1][1] = ((nor[1] * nor[1]) * ico) + co;
780         mat[1][2] = ((nor[1] * nor[2]) * ico) + nsi[0];
781         mat[2][0] = ((nor[0] * nor[2]) * ico) + nsi[1];
782         mat[2][1] = ((nor[1] * nor[2]) * ico) - nsi[0];
783         mat[2][2] = ((nor[2] * nor[2]) * ico) + co;
784 }
785
786 /* axis angle to 4x4 matrix - safer version (normalization of axis performed) */
787 void axis_angle_to_mat4(float mat[4][4], const float axis[3], const float angle)
788 {
789         float tmat[3][3];
790
791         axis_angle_to_mat3(tmat, axis, angle);
792         unit_m4(mat);
793         copy_m4_m3(mat, tmat);
794 }
795
796 /* 3x3 matrix to axis angle (see Mat4ToVecRot too) */
797 void mat3_to_axis_angle(float axis[3], float *angle, float mat[3][3])
798 {
799         float q[4];
800
801         /* use quaternions as intermediate representation */
802         /* TODO: it would be nicer to go straight there... */
803         mat3_to_quat(q, mat);
804         quat_to_axis_angle(axis, angle, q);
805 }
806
807 /* 4x4 matrix to axis angle (see Mat4ToVecRot too) */
808 void mat4_to_axis_angle(float axis[3], float *angle, float mat[4][4])
809 {
810         float q[4];
811
812         /* use quaternions as intermediate representation */
813         /* TODO: it would be nicer to go straight there... */
814         mat4_to_quat(q, mat);
815         quat_to_axis_angle(axis, angle, q);
816 }
817
818 void single_axis_angle_to_mat3(float mat[3][3], const char axis, const float angle)
819 {
820         const float angle_cos = cosf(angle);
821         const float angle_sin = sinf(angle);
822
823         switch (axis) {
824                 case 'X': /* rotation around X */
825                         mat[0][0] = 1.0f;
826                         mat[0][1] = 0.0f;
827                         mat[0][2] = 0.0f;
828                         mat[1][0] = 0.0f;
829                         mat[1][1] = angle_cos;
830                         mat[1][2] = angle_sin;
831                         mat[2][0] = 0.0f;
832                         mat[2][1] = -angle_sin;
833                         mat[2][2] = angle_cos;
834                         break;
835                 case 'Y': /* rotation around Y */
836                         mat[0][0] = angle_cos;
837                         mat[0][1] = 0.0f;
838                         mat[0][2] = -angle_sin;
839                         mat[1][0] = 0.0f;
840                         mat[1][1] = 1.0f;
841                         mat[1][2] = 0.0f;
842                         mat[2][0] = angle_sin;
843                         mat[2][1] = 0.0f;
844                         mat[2][2] = angle_cos;
845                         break;
846                 case 'Z': /* rotation around Z */
847                         mat[0][0] = angle_cos;
848                         mat[0][1] = angle_sin;
849                         mat[0][2] = 0.0f;
850                         mat[1][0] = -angle_sin;
851                         mat[1][1] = angle_cos;
852                         mat[1][2] = 0.0f;
853                         mat[2][0] = 0.0f;
854                         mat[2][1] = 0.0f;
855                         mat[2][2] = 1.0f;
856                         break;
857                 default:
858                         assert(0);
859         }
860 }
861
862 /****************************** Vector/Rotation ******************************/
863 /* TODO: the following calls should probably be deprecated sometime         */
864
865 /* TODO, replace use of this function with axis_angle_to_mat3() */
866 void vec_rot_to_mat3(float mat[3][3], const float vec[3], const float phi)
867 {
868         /* rotation of phi radials around vec */
869         float vx, vx2, vy, vy2, vz, vz2, co, si;
870
871         vx = vec[0];
872         vy = vec[1];
873         vz = vec[2];
874         vx2 = vx * vx;
875         vy2 = vy * vy;
876         vz2 = vz * vz;
877         co = cosf(phi);
878         si = sinf(phi);
879
880         mat[0][0] = vx2 + co * (1.0f - vx2);
881         mat[0][1] = vx * vy * (1.0f - co) + vz * si;
882         mat[0][2] = vz * vx * (1.0f - co) - vy * si;
883         mat[1][0] = vx * vy * (1.0f - co) - vz * si;
884         mat[1][1] = vy2 + co * (1.0f - vy2);
885         mat[1][2] = vy * vz * (1.0f - co) + vx * si;
886         mat[2][0] = vz * vx * (1.0f - co) + vy * si;
887         mat[2][1] = vy * vz * (1.0f - co) - vx * si;
888         mat[2][2] = vz2 + co * (1.0f - vz2);
889 }
890
891 /******************************** XYZ Eulers *********************************/
892
893 /* XYZ order */
894 void eul_to_mat3(float mat[3][3], const float eul[3])
895 {
896         double ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
897
898         ci = cos(eul[0]);
899         cj = cos(eul[1]);
900         ch = cos(eul[2]);
901         si = sin(eul[0]);
902         sj = sin(eul[1]);
903         sh = sin(eul[2]);
904         cc = ci * ch;
905         cs = ci * sh;
906         sc = si * ch;
907         ss = si * sh;
908
909         mat[0][0] = (float)(cj * ch);
910         mat[1][0] = (float)(sj * sc - cs);
911         mat[2][0] = (float)(sj * cc + ss);
912         mat[0][1] = (float)(cj * sh);
913         mat[1][1] = (float)(sj * ss + cc);
914         mat[2][1] = (float)(sj * cs - sc);
915         mat[0][2] = (float)-sj;
916         mat[1][2] = (float)(cj * si);
917         mat[2][2] = (float)(cj * ci);
918
919 }
920
921 /* XYZ order */
922 void eul_to_mat4(float mat[4][4], const float eul[3])
923 {
924         double ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
925
926         ci = cos(eul[0]);
927         cj = cos(eul[1]);
928         ch = cos(eul[2]);
929         si = sin(eul[0]);
930         sj = sin(eul[1]);
931         sh = sin(eul[2]);
932         cc = ci * ch;
933         cs = ci * sh;
934         sc = si * ch;
935         ss = si * sh;
936
937         mat[0][0] = (float)(cj * ch);
938         mat[1][0] = (float)(sj * sc - cs);
939         mat[2][0] = (float)(sj * cc + ss);
940         mat[0][1] = (float)(cj * sh);
941         mat[1][1] = (float)(sj * ss + cc);
942         mat[2][1] = (float)(sj * cs - sc);
943         mat[0][2] = (float)-sj;
944         mat[1][2] = (float)(cj * si);
945         mat[2][2] = (float)(cj * ci);
946
947
948         mat[3][0] = mat[3][1] = mat[3][2] = mat[0][3] = mat[1][3] = mat[2][3] = 0.0f;
949         mat[3][3] = 1.0f;
950 }
951
952 /* returns two euler calculation methods, so we can pick the best */
953
954 /* XYZ order */
955 static void mat3_to_eul2(float tmat[3][3], float eul1[3], float eul2[3])
956 {
957         float cy, quat[4], mat[3][3];
958
959         mat3_to_quat(quat, tmat);
960         quat_to_mat3(mat, quat);
961         copy_m3_m3(mat, tmat);
962         normalize_m3(mat);
963
964         cy = (float)sqrt(mat[0][0] * mat[0][0] + mat[0][1] * mat[0][1]);
965
966         if (cy > 16.0f * FLT_EPSILON) {
967
968                 eul1[0] = (float)atan2(mat[1][2], mat[2][2]);
969                 eul1[1] = (float)atan2(-mat[0][2], cy);
970                 eul1[2] = (float)atan2(mat[0][1], mat[0][0]);
971
972                 eul2[0] = (float)atan2(-mat[1][2], -mat[2][2]);
973                 eul2[1] = (float)atan2(-mat[0][2], -cy);
974                 eul2[2] = (float)atan2(-mat[0][1], -mat[0][0]);
975
976         }
977         else {
978                 eul1[0] = (float)atan2(-mat[2][1], mat[1][1]);
979                 eul1[1] = (float)atan2(-mat[0][2], cy);
980                 eul1[2] = 0.0f;
981
982                 copy_v3_v3(eul2, eul1);
983         }
984 }
985
986 /* XYZ order */
987 void mat3_to_eul(float *eul, float tmat[3][3])
988 {
989         float eul1[3], eul2[3];
990
991         mat3_to_eul2(tmat, eul1, eul2);
992
993         /* return best, which is just the one with lowest values it in */
994         if (fabsf(eul1[0]) + fabsf(eul1[1]) + fabsf(eul1[2]) > fabsf(eul2[0]) + fabsf(eul2[1]) + fabsf(eul2[2])) {
995                 copy_v3_v3(eul, eul2);
996         }
997         else {
998                 copy_v3_v3(eul, eul1);
999         }
1000 }
1001
1002 /* XYZ order */
1003 void mat4_to_eul(float *eul, float tmat[4][4])
1004 {
1005         float tempMat[3][3];
1006
1007         copy_m3_m4(tempMat, tmat);
1008         normalize_m3(tempMat);
1009         mat3_to_eul(eul, tempMat);
1010 }
1011
1012 /* XYZ order */
1013 void quat_to_eul(float *eul, const float quat[4])
1014 {
1015         float mat[3][3];
1016
1017         quat_to_mat3(mat, quat);
1018         mat3_to_eul(eul, mat);
1019 }
1020
1021 /* XYZ order */
1022 void eul_to_quat(float *quat, const float eul[3])
1023 {
1024         float ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
1025
1026         ti = eul[0] * 0.5f;
1027         tj = eul[1] * 0.5f;
1028         th = eul[2] * 0.5f;
1029         ci = cosf(ti);
1030         cj = cosf(tj);
1031         ch = cosf(th);
1032         si = sinf(ti);
1033         sj = sinf(tj);
1034         sh = sinf(th);
1035         cc = ci * ch;
1036         cs = ci * sh;
1037         sc = si * ch;
1038         ss = si * sh;
1039
1040         quat[0] = cj * cc + sj * ss;
1041         quat[1] = cj * sc - sj * cs;
1042         quat[2] = cj * ss + sj * cc;
1043         quat[3] = cj * cs - sj * sc;
1044 }
1045
1046 /* XYZ order */
1047 void rotate_eul(float *beul, const char axis, const float ang)
1048 {
1049         float eul[3], mat1[3][3], mat2[3][3], totmat[3][3];
1050
1051         assert(axis >= 'X' && axis <= 'Z');
1052
1053         eul[0] = eul[1] = eul[2] = 0.0f;
1054         if (axis == 'X') eul[0] = ang;
1055         else if (axis == 'Y') eul[1] = ang;
1056         else eul[2] = ang;
1057
1058         eul_to_mat3(mat1, eul);
1059         eul_to_mat3(mat2, beul);
1060
1061         mul_m3_m3m3(totmat, mat2, mat1);
1062
1063         mat3_to_eul(beul, totmat);
1064
1065 }
1066
1067 /* order independent! */
1068 void compatible_eul(float eul[3], const float oldrot[3])
1069 {
1070         /* we could use M_PI as pi_thresh: which is correct but 5.1 gives better results.
1071          * Checked with baking actions to fcurves - campbell */
1072         const float pi_thresh = (5.1f);
1073         const float pi_x2     = (2.0f * (float)M_PI);
1074
1075         float deul[3];
1076         unsigned int i;
1077
1078         /* correct differences of about 360 degrees first */
1079         for (i = 0; i < 3; i++) {
1080                 deul[i] = eul[i] - oldrot[i];
1081                 if (deul[i] > pi_thresh) {
1082                         eul[i] -= floorf(( deul[i] / pi_x2) + 0.5f) * pi_x2;
1083                         deul[i] = eul[i] - oldrot[i];
1084                 }
1085                 else if (deul[i] < -pi_thresh) {
1086                         eul[i] += floorf((-deul[i] / pi_x2) + 0.5f) * pi_x2;
1087                         deul[i] = eul[i] - oldrot[i];
1088                 }
1089         }
1090
1091         /* is 1 of the axis rotations larger than 180 degrees and the other small? NO ELSE IF!! */
1092         if (fabsf(deul[0]) > 3.2f && fabsf(deul[1]) < 1.6f && fabsf(deul[2]) < 1.6f) {
1093                 if (deul[0] > 0.0f) eul[0] -= pi_x2;
1094                 else                eul[0] += pi_x2;
1095         }
1096         if (fabsf(deul[1]) > 3.2f && fabsf(deul[2]) < 1.6f && fabsf(deul[0]) < 1.6f) {
1097                 if (deul[1] > 0.0f) eul[1] -= pi_x2;
1098                 else                eul[1] += pi_x2;
1099         }
1100         if (fabsf(deul[2]) > 3.2f && fabsf(deul[0]) < 1.6f && fabsf(deul[1]) < 1.6f) {
1101                 if (deul[2] > 0.0f) eul[2] -= pi_x2;
1102                 else                eul[2] += pi_x2;
1103         }
1104
1105 #undef PI_THRESH
1106 #undef PI_2F
1107 }
1108
1109 /* uses 2 methods to retrieve eulers, and picks the closest */
1110
1111 /* XYZ order */
1112 void mat3_to_compatible_eul(float eul[3], const float oldrot[3], float mat[3][3])
1113 {
1114         float eul1[3], eul2[3];
1115         float d1, d2;
1116
1117         mat3_to_eul2(mat, eul1, eul2);
1118
1119         compatible_eul(eul1, oldrot);
1120         compatible_eul(eul2, oldrot);
1121
1122         d1 = fabsf(eul1[0] - oldrot[0]) + fabsf(eul1[1] - oldrot[1]) + fabsf(eul1[2] - oldrot[2]);
1123         d2 = fabsf(eul2[0] - oldrot[0]) + fabsf(eul2[1] - oldrot[1]) + fabsf(eul2[2] - oldrot[2]);
1124
1125         /* return best, which is just the one with lowest difference */
1126         if (d1 > d2) {
1127                 copy_v3_v3(eul, eul2);
1128         }
1129         else {
1130                 copy_v3_v3(eul, eul1);
1131         }
1132
1133 }
1134
1135 /************************** Arbitrary Order Eulers ***************************/
1136
1137 /* Euler Rotation Order Code:
1138  * was adapted from
1139  *      ANSI C code from the article
1140  *      "Euler Angle Conversion"
1141  *      by Ken Shoemake, shoemake@graphics.cis.upenn.edu
1142  *      in "Graphics Gems IV", Academic Press, 1994
1143  * for use in Blender
1144  */
1145
1146 /* Type for rotation order info - see wiki for derivation details */
1147 typedef struct RotOrderInfo {
1148         short axis[3];
1149         short parity; /* parity of axis permutation (even=0, odd=1) - 'n' in original code */
1150 } RotOrderInfo;
1151
1152 /* Array of info for Rotation Order calculations
1153  * WARNING: must be kept in same order as eEulerRotationOrders
1154  */
1155 static const RotOrderInfo rotOrders[] = {
1156         /* i, j, k, n */
1157         {{0, 1, 2}, 0}, /* XYZ */
1158         {{0, 2, 1}, 1}, /* XZY */
1159         {{1, 0, 2}, 1}, /* YXZ */
1160         {{1, 2, 0}, 0}, /* YZX */
1161         {{2, 0, 1}, 0}, /* ZXY */
1162         {{2, 1, 0}, 1}  /* ZYX */
1163 };
1164
1165 /* Get relevant pointer to rotation order set from the array
1166  * NOTE: since we start at 1 for the values, but arrays index from 0,
1167  *               there is -1 factor involved in this process...
1168  */
1169 #define GET_ROTATIONORDER_INFO(order) (assert(order >= 0 && order <= 6), (order < 1) ? &rotOrders[0] : &rotOrders[(order) - 1])
1170
1171 /* Construct quaternion from Euler angles (in radians). */
1172 void eulO_to_quat(float q[4], const float e[3], const short order)
1173 {
1174         const RotOrderInfo *R = GET_ROTATIONORDER_INFO(order);
1175         short i = R->axis[0], j = R->axis[1], k = R->axis[2];
1176         double ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
1177         double a[3];
1178
1179         ti = e[i] * 0.5f;
1180         tj = e[j] * (R->parity ? -0.5f : 0.5f);
1181         th = e[k] * 0.5f;
1182
1183         ci = cos(ti);
1184         cj = cos(tj);
1185         ch = cos(th);
1186         si = sin(ti);
1187         sj = sin(tj);
1188         sh = sin(th);
1189
1190         cc = ci * ch;
1191         cs = ci * sh;
1192         sc = si * ch;
1193         ss = si * sh;
1194
1195         a[i] = cj * sc - sj * cs;
1196         a[j] = cj * ss + sj * cc;
1197         a[k] = cj * cs - sj * sc;
1198
1199         q[0] = cj * cc + sj * ss;
1200         q[1] = a[0];
1201         q[2] = a[1];
1202         q[3] = a[2];
1203
1204         if (R->parity) q[j + 1] = -q[j + 1];
1205 }
1206
1207 /* Convert quaternion to Euler angles (in radians). */
1208 void quat_to_eulO(float e[3], short const order, const float q[4])
1209 {
1210         float M[3][3];
1211
1212         quat_to_mat3(M, q);
1213         mat3_to_eulO(e, order, M);
1214 }
1215
1216 /* Construct 3x3 matrix from Euler angles (in radians). */
1217 void eulO_to_mat3(float M[3][3], const float e[3], const short order)
1218 {
1219         const RotOrderInfo *R = GET_ROTATIONORDER_INFO(order);
1220         short i = R->axis[0], j = R->axis[1], k = R->axis[2];
1221         double ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
1222
1223         if (R->parity) {
1224                 ti = -e[i];
1225                 tj = -e[j];
1226                 th = -e[k];
1227         }
1228         else {
1229                 ti = e[i];
1230                 tj = e[j];
1231                 th = e[k];
1232         }
1233
1234         ci = cos(ti);
1235         cj = cos(tj);
1236         ch = cos(th);
1237         si = sin(ti);
1238         sj = sin(tj);
1239         sh = sin(th);
1240
1241         cc = ci * ch;
1242         cs = ci * sh;
1243         sc = si * ch;
1244         ss = si * sh;
1245
1246         M[i][i] = cj * ch;
1247         M[j][i] = sj * sc - cs;
1248         M[k][i] = sj * cc + ss;
1249         M[i][j] = cj * sh;
1250         M[j][j] = sj * ss + cc;
1251         M[k][j] = sj * cs - sc;
1252         M[i][k] = -sj;
1253         M[j][k] = cj * si;
1254         M[k][k] = cj * ci;
1255 }
1256
1257 /* returns two euler calculation methods, so we can pick the best */
1258 static void mat3_to_eulo2(float M[3][3], float *e1, float *e2, const short order)
1259 {
1260         const RotOrderInfo *R = GET_ROTATIONORDER_INFO(order);
1261         short i = R->axis[0], j = R->axis[1], k = R->axis[2];
1262         float m[3][3];
1263         double cy;
1264
1265         /* process the matrix first */
1266         copy_m3_m3(m, M);
1267         normalize_m3(m);
1268
1269         cy = sqrt(m[i][i] * m[i][i] + m[i][j] * m[i][j]);
1270
1271         if (cy > 16.0 * (double)FLT_EPSILON) {
1272                 e1[i] = atan2(m[j][k], m[k][k]);
1273                 e1[j] = atan2(-m[i][k], cy);
1274                 e1[k] = atan2(m[i][j], m[i][i]);
1275
1276                 e2[i] = atan2(-m[j][k], -m[k][k]);
1277                 e2[j] = atan2(-m[i][k], -cy);
1278                 e2[k] = atan2(-m[i][j], -m[i][i]);
1279         }
1280         else {
1281                 e1[i] = atan2(-m[k][j], m[j][j]);
1282                 e1[j] = atan2(-m[i][k], cy);
1283                 e1[k] = 0;
1284
1285                 copy_v3_v3(e2, e1);
1286         }
1287
1288         if (R->parity) {
1289                 e1[0] = -e1[0];
1290                 e1[1] = -e1[1];
1291                 e1[2] = -e1[2];
1292
1293                 e2[0] = -e2[0];
1294                 e2[1] = -e2[1];
1295                 e2[2] = -e2[2];
1296         }
1297 }
1298
1299 /* Construct 4x4 matrix from Euler angles (in radians). */
1300 void eulO_to_mat4(float M[4][4], const float e[3], const short order)
1301 {
1302         float m[3][3];
1303
1304         /* for now, we'll just do this the slow way (i.e. copying matrices) */
1305         normalize_m3(m);
1306         eulO_to_mat3(m, e, order);
1307         copy_m4_m3(M, m);
1308 }
1309
1310 /* Convert 3x3 matrix to Euler angles (in radians). */
1311 void mat3_to_eulO(float eul[3], const short order, float M[3][3])
1312 {
1313         float eul1[3], eul2[3];
1314
1315         mat3_to_eulo2(M, eul1, eul2, order);
1316
1317         /* return best, which is just the one with lowest values it in */
1318         if (fabsf(eul1[0]) + fabsf(eul1[1]) + fabsf(eul1[2]) > fabsf(eul2[0]) + fabsf(eul2[1]) + fabsf(eul2[2])) {
1319                 copy_v3_v3(eul, eul2);
1320         }
1321         else {
1322                 copy_v3_v3(eul, eul1);
1323         }
1324 }
1325
1326 /* Convert 4x4 matrix to Euler angles (in radians). */
1327 void mat4_to_eulO(float e[3], const short order, float M[4][4])
1328 {
1329         float m[3][3];
1330
1331         /* for now, we'll just do this the slow way (i.e. copying matrices) */
1332         copy_m3_m4(m, M);
1333         normalize_m3(m);
1334         mat3_to_eulO(e, order, m);
1335 }
1336
1337 /* uses 2 methods to retrieve eulers, and picks the closest */
1338 void mat3_to_compatible_eulO(float eul[3], float oldrot[3], const short order, float mat[3][3])
1339 {
1340         float eul1[3], eul2[3];
1341         float d1, d2;
1342
1343         mat3_to_eulo2(mat, eul1, eul2, order);
1344
1345         compatible_eul(eul1, oldrot);
1346         compatible_eul(eul2, oldrot);
1347
1348         d1 = fabsf(eul1[0] - oldrot[0]) + fabsf(eul1[1] - oldrot[1]) + fabsf(eul1[2] - oldrot[2]);
1349         d2 = fabsf(eul2[0] - oldrot[0]) + fabsf(eul2[1] - oldrot[1]) + fabsf(eul2[2] - oldrot[2]);
1350
1351         /* return best, which is just the one with lowest difference */
1352         if (d1 > d2)
1353                 copy_v3_v3(eul, eul2);
1354         else
1355                 copy_v3_v3(eul, eul1);
1356 }
1357
1358 void mat4_to_compatible_eulO(float eul[3], float oldrot[3], const short order, float M[4][4])
1359 {
1360         float m[3][3];
1361
1362         /* for now, we'll just do this the slow way (i.e. copying matrices) */
1363         copy_m3_m4(m, M);
1364         normalize_m3(m);
1365         mat3_to_compatible_eulO(eul, oldrot, order, m);
1366 }
1367 /* rotate the given euler by the given angle on the specified axis */
1368 /* NOTE: is this safe to do with different axis orders? */
1369
1370 void rotate_eulO(float beul[3], const short order, char axis, float ang)
1371 {
1372         float eul[3], mat1[3][3], mat2[3][3], totmat[3][3];
1373
1374         assert(axis >= 'X' && axis <= 'Z');
1375
1376         eul[0] = eul[1] = eul[2] = 0.0f;
1377         if (axis == 'X')
1378                 eul[0] = ang;
1379         else if (axis == 'Y')
1380                 eul[1] = ang;
1381         else
1382                 eul[2] = ang;
1383
1384         eulO_to_mat3(mat1, eul, order);
1385         eulO_to_mat3(mat2, beul, order);
1386
1387         mul_m3_m3m3(totmat, mat2, mat1);
1388
1389         mat3_to_eulO(beul, order, totmat);
1390 }
1391
1392 /* the matrix is written to as 3 axis vectors */
1393 void eulO_to_gimbal_axis(float gmat[3][3], const float eul[3], const short order)
1394 {
1395         const RotOrderInfo *R = GET_ROTATIONORDER_INFO(order);
1396
1397         float mat[3][3];
1398         float teul[3];
1399
1400         /* first axis is local */
1401         eulO_to_mat3(mat, eul, order);
1402         copy_v3_v3(gmat[R->axis[0]], mat[R->axis[0]]);
1403
1404         /* second axis is local minus first rotation */
1405         copy_v3_v3(teul, eul);
1406         teul[R->axis[0]] = 0;
1407         eulO_to_mat3(mat, teul, order);
1408         copy_v3_v3(gmat[R->axis[1]], mat[R->axis[1]]);
1409
1410
1411         /* Last axis is global */
1412         gmat[R->axis[2]][0] = 0;
1413         gmat[R->axis[2]][1] = 0;
1414         gmat[R->axis[2]][2] = 0;
1415         gmat[R->axis[2]][R->axis[2]] = 1;
1416 }
1417
1418 /******************************* Dual Quaternions ****************************/
1419
1420 /**
1421  * Conversion routines between (regular quaternion, translation) and
1422  * dual quaternion.
1423  *
1424  * Version 1.0.0, February 7th, 2007
1425  *
1426  * Copyright (C) 2006-2007 University of Dublin, Trinity College, All Rights
1427  * Reserved
1428  *
1429  * This software is provided 'as-is', without any express or implied
1430  * warranty.  In no event will the author(s) be held liable for any damages
1431  * arising from the use of this software.
1432  *
1433  * Permission is granted to anyone to use this software for any purpose,
1434  * including commercial applications, and to alter it and redistribute it
1435  * freely, subject to the following restrictions:
1436  *
1437  * 1. The origin of this software must not be misrepresented; you must not
1438  *    claim that you wrote the original software. If you use this software
1439  *    in a product, an acknowledgment in the product documentation would be
1440  *    appreciated but is not required.
1441  * 2. Altered source versions must be plainly marked as such, and must not be
1442  *    misrepresented as being the original software.
1443  * 3. This notice may not be removed or altered from any source distribution.
1444  *
1445  * \author Ladislav Kavan, kavanl@cs.tcd.ie
1446  *
1447  * Changes for Blender:
1448  * - renaming, style changes and optimization's
1449  * - added support for scaling
1450  */
1451
1452 void mat4_to_dquat(DualQuat *dq, float basemat[4][4], float mat[4][4])
1453 {
1454         float *t, *q, dscale[3], scale[3], basequat[4];
1455         float baseRS[4][4], baseinv[4][4], baseR[4][4], baseRinv[4][4];
1456         float R[4][4], S[4][4];
1457
1458         /* split scaling and rotation, there is probably a faster way to do
1459          * this, it's done like this now to correctly get negative scaling */
1460         mult_m4_m4m4(baseRS, mat, basemat);
1461         mat4_to_size(scale, baseRS);
1462
1463         dscale[0] = scale[0] - 1.0f;
1464         dscale[1] = scale[1] - 1.0f;
1465         dscale[2] = scale[2] - 1.0f;
1466
1467         if ((determinant_m4(mat) < 0.0f) || len_v3(dscale) > 1e-4f) {
1468                 /* extract R and S  */
1469                 float tmp[4][4];
1470
1471                 /* extra orthogonalize, to avoid flipping with stretched bones */
1472                 copy_m4_m4(tmp, baseRS);
1473                 orthogonalize_m4(tmp, 1);
1474                 mat4_to_quat(basequat, tmp);
1475
1476                 quat_to_mat4(baseR, basequat);
1477                 copy_v3_v3(baseR[3], baseRS[3]);
1478
1479                 invert_m4_m4(baseinv, basemat);
1480                 mult_m4_m4m4(R, baseR, baseinv);
1481
1482                 invert_m4_m4(baseRinv, baseR);
1483                 mult_m4_m4m4(S, baseRinv, baseRS);
1484
1485                 /* set scaling part */
1486                 mul_serie_m4(dq->scale, basemat, S, baseinv, NULL, NULL, NULL, NULL, NULL);
1487                 dq->scale_weight = 1.0f;
1488         }
1489         else {
1490                 /* matrix does not contain scaling */
1491                 copy_m4_m4(R, mat);
1492                 dq->scale_weight = 0.0f;
1493         }
1494
1495         /* non-dual part */
1496         mat4_to_quat(dq->quat, R);
1497
1498         /* dual part */
1499         t = R[3];
1500         q = dq->quat;
1501         dq->trans[0] = -0.5f * ( t[0] * q[1] + t[1] * q[2] + t[2] * q[3]);
1502         dq->trans[1] =  0.5f * ( t[0] * q[0] + t[1] * q[3] - t[2] * q[2]);
1503         dq->trans[2] =  0.5f * (-t[0] * q[3] + t[1] * q[0] + t[2] * q[1]);
1504         dq->trans[3] =  0.5f * ( t[0] * q[2] - t[1] * q[1] + t[2] * q[0]);
1505 }
1506
1507 void dquat_to_mat4(float mat[4][4], DualQuat *dq)
1508 {
1509         float len, *t, q0[4];
1510
1511         /* regular quaternion */
1512         copy_qt_qt(q0, dq->quat);
1513
1514         /* normalize */
1515         len = sqrtf(dot_qtqt(q0, q0));
1516         if (len != 0.0f)
1517                 mul_qt_fl(q0, 1.0f / len);
1518
1519         /* rotation */
1520         quat_to_mat4(mat, q0);
1521
1522         /* translation */
1523         t = dq->trans;
1524         mat[3][0] = 2.0f * (-t[0] * q0[1] + t[1] * q0[0] - t[2] * q0[3] + t[3] * q0[2]);
1525         mat[3][1] = 2.0f * (-t[0] * q0[2] + t[1] * q0[3] + t[2] * q0[0] - t[3] * q0[1]);
1526         mat[3][2] = 2.0f * (-t[0] * q0[3] - t[1] * q0[2] + t[2] * q0[1] + t[3] * q0[0]);
1527
1528         /* note: this does not handle scaling */
1529 }
1530
1531 void add_weighted_dq_dq(DualQuat *dqsum, DualQuat *dq, float weight)
1532 {
1533         int flipped = 0;
1534
1535         /* make sure we interpolate quats in the right direction */
1536         if (dot_qtqt(dq->quat, dqsum->quat) < 0) {
1537                 flipped = 1;
1538                 weight = -weight;
1539         }
1540
1541         /* interpolate rotation and translation */
1542         dqsum->quat[0] += weight * dq->quat[0];
1543         dqsum->quat[1] += weight * dq->quat[1];
1544         dqsum->quat[2] += weight * dq->quat[2];
1545         dqsum->quat[3] += weight * dq->quat[3];
1546
1547         dqsum->trans[0] += weight * dq->trans[0];
1548         dqsum->trans[1] += weight * dq->trans[1];
1549         dqsum->trans[2] += weight * dq->trans[2];
1550         dqsum->trans[3] += weight * dq->trans[3];
1551
1552         /* interpolate scale - but only if needed */
1553         if (dq->scale_weight) {
1554                 float wmat[4][4];
1555
1556                 if (flipped) /* we don't want negative weights for scaling */
1557                         weight = -weight;
1558
1559                 copy_m4_m4(wmat, dq->scale);
1560                 mul_m4_fl(wmat, weight);
1561                 add_m4_m4m4(dqsum->scale, dqsum->scale, wmat);
1562                 dqsum->scale_weight += weight;
1563         }
1564 }
1565
1566 void normalize_dq(DualQuat *dq, float totweight)
1567 {
1568         float scale = 1.0f / totweight;
1569
1570         mul_qt_fl(dq->quat, scale);
1571         mul_qt_fl(dq->trans, scale);
1572
1573         if (dq->scale_weight) {
1574                 float addweight = totweight - dq->scale_weight;
1575
1576                 if (addweight) {
1577                         dq->scale[0][0] += addweight;
1578                         dq->scale[1][1] += addweight;
1579                         dq->scale[2][2] += addweight;
1580                         dq->scale[3][3] += addweight;
1581                 }
1582
1583                 mul_m4_fl(dq->scale, scale);
1584                 dq->scale_weight = 1.0f;
1585         }
1586 }
1587
1588 void mul_v3m3_dq(float co[3], float mat[3][3], DualQuat *dq)
1589 {
1590         float M[3][3], t[3], scalemat[3][3], len2;
1591         float w = dq->quat[0], x = dq->quat[1], y = dq->quat[2], z = dq->quat[3];
1592         float t0 = dq->trans[0], t1 = dq->trans[1], t2 = dq->trans[2], t3 = dq->trans[3];
1593
1594         /* rotation matrix */
1595         M[0][0] = w * w + x * x - y * y - z * z;
1596         M[1][0] = 2 * (x * y - w * z);
1597         M[2][0] = 2 * (x * z + w * y);
1598
1599         M[0][1] = 2 * (x * y + w * z);
1600         M[1][1] = w * w + y * y - x * x - z * z;
1601         M[2][1] = 2 * (y * z - w * x);
1602
1603         M[0][2] = 2 * (x * z - w * y);
1604         M[1][2] = 2 * (y * z + w * x);
1605         M[2][2] = w * w + z * z - x * x - y * y;
1606
1607         len2 = dot_qtqt(dq->quat, dq->quat);
1608         if (len2 > 0.0f)
1609                 len2 = 1.0f / len2;
1610
1611         /* translation */
1612         t[0] = 2 * (-t0 * x + w * t1 - t2 * z + y * t3);
1613         t[1] = 2 * (-t0 * y + t1 * z - x * t3 + w * t2);
1614         t[2] = 2 * (-t0 * z + x * t2 + w * t3 - t1 * y);
1615
1616         /* apply scaling */
1617         if (dq->scale_weight)
1618                 mul_m4_v3(dq->scale, co);
1619
1620         /* apply rotation and translation */
1621         mul_m3_v3(M, co);
1622         co[0] = (co[0] + t[0]) * len2;
1623         co[1] = (co[1] + t[1]) * len2;
1624         co[2] = (co[2] + t[2]) * len2;
1625
1626         /* compute crazyspace correction mat */
1627         if (mat) {
1628                 if (dq->scale_weight) {
1629                         copy_m3_m4(scalemat, dq->scale);
1630                         mul_m3_m3m3(mat, M, scalemat);
1631                 }
1632                 else
1633                         copy_m3_m3(mat, M);
1634                 mul_m3_fl(mat, len2);
1635         }
1636 }
1637
1638 void copy_dq_dq(DualQuat *dq1, DualQuat *dq2)
1639 {
1640         memcpy(dq1, dq2, sizeof(DualQuat));
1641 }
1642
1643 /* axis matches eTrackToAxis_Modes */
1644 void quat_apply_track(float quat[4], short axis, short upflag)
1645 {
1646         /* rotations are hard coded to match vec_to_quat */
1647         const float quat_track[][4] = {
1648                 {M_SQRT1_2, 0.0, -M_SQRT1_2, 0.0}, /* pos-y90 */
1649                 {0.5, 0.5, 0.5, 0.5}, /* Quaternion((1,0,0), radians(90)) * Quaternion((0,1,0), radians(90)) */
1650                 {M_SQRT1_2, 0.0, 0.0, M_SQRT1_2}, /* pos-z90 */
1651                 {M_SQRT1_2, 0.0, M_SQRT1_2, 0.0}, /* neg-y90 */
1652                 {0.5, -0.5, -0.5, 0.5}, /* Quaternion((1,0,0), radians(-90)) * Quaternion((0,1,0), radians(-90)) */
1653                 {0.0, M_SQRT1_2, M_SQRT1_2, 0.0} /* no rotation */
1654         };
1655
1656         assert(axis >= 0 && axis <= 5);
1657         assert(upflag >= 0 && upflag <= 2);
1658
1659         mul_qt_qtqt(quat, quat, quat_track[axis]);
1660
1661         if (axis > 2)
1662                 axis = axis - 3;
1663
1664         /* there are 2 possible up-axis for each axis used, the 'quat_track' applies so the first
1665          * up axis is used X->Y, Y->X, Z->X, if this first up axis isn't used then rotate 90d
1666          * the strange bit shift below just find the low axis {X:Y, Y:X, Z:X} */
1667         if (upflag != (2 - axis) >> 1) {
1668                 float q[4] = {M_SQRT1_2, 0.0, 0.0, 0.0}; /* assign 90d rotation axis */
1669                 q[axis + 1] = ((axis == 1)) ? M_SQRT1_2 : -M_SQRT1_2; /* flip non Y axis */
1670                 mul_qt_qtqt(quat, quat, q);
1671         }
1672 }
1673
1674 void vec_apply_track(float vec[3], short axis)
1675 {
1676         float tvec[3];
1677
1678         assert(axis >= 0 && axis <= 5);
1679
1680         copy_v3_v3(tvec, vec);
1681
1682         switch (axis) {
1683                 case 0: /* pos-x */
1684                         /* vec[0] =  0.0; */
1685                         vec[1] = tvec[2];
1686                         vec[2] = -tvec[1];
1687                         break;
1688                 case 1: /* pos-y */
1689                         /* vec[0] = tvec[0]; */
1690                         /* vec[1] =  0.0; */
1691                         /* vec[2] = tvec[2]; */
1692                         break;
1693                 case 2: /* pos-z */
1694                         /* vec[0] = tvec[0]; */
1695                         /* vec[1] = tvec[1]; */
1696                         /* vec[2] =  0.0; */
1697                         break;
1698                 case 3: /* neg-x */
1699                         /* vec[0] =  0.0; */
1700                         vec[1] = tvec[2];
1701                         vec[2] = -tvec[1];
1702                         break;
1703                 case 4: /* neg-y */
1704                         vec[0] = -tvec[2];
1705                         /* vec[1] =  0.0; */
1706                         vec[2] = tvec[0];
1707                         break;
1708                 case 5: /* neg-z */
1709                         vec[0] = -tvec[0];
1710                         vec[1] = -tvec[1];
1711                         /* vec[2] =  0.0; */
1712                         break;
1713         }
1714 }
1715
1716 /* lens/angle conversion (radians) */
1717 float focallength_to_fov(float focal_length, float sensor)
1718 {
1719         return 2.0f * atanf((sensor / 2.0f) / focal_length);
1720 }
1721
1722 float fov_to_focallength(float hfov, float sensor)
1723 {
1724         return (sensor / 2.0f) / tanf(hfov * 0.5f);
1725 }
1726
1727 /* 'mod_inline(-3,4)= 1', 'fmod(-3,4)= -3' */
1728 static float mod_inline(float a, float b)
1729 {
1730         return a - (b * floorf(a / b));
1731 }
1732
1733 float angle_wrap_rad(float angle)
1734 {
1735         return mod_inline(angle + (float)M_PI, (float)M_PI * 2.0f) - (float)M_PI;
1736 }
1737
1738 float angle_wrap_deg(float angle)
1739 {
1740         return mod_inline(angle + 180.0f, 360.0f) - 180.0f;
1741 }
1742
1743 /* returns an angle compatible with angle_compat */
1744 float angle_compat_rad(float angle, float angle_compat)
1745 {
1746         return angle + (floorf(((angle_compat - angle) / (float)M_PI) + 0.5f)) * (float)M_PI;
1747 }
1748
1749 /* axis conversion */
1750 static float _axis_convert_matrix[23][3][3] = {
1751         {{-1.0, 0.0, 0.0}, {0.0, -1.0, 0.0}, {0.0, 0.0, 1.0}},
1752         {{-1.0, 0.0, 0.0}, {0.0, 0.0, -1.0}, {0.0, -1.0, 0.0}},
1753         {{-1.0, 0.0, 0.0}, {0.0, 0.0, 1.0}, {0.0, 1.0, 0.0}},
1754         {{-1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, -1.0}},
1755         {{0.0, -1.0, 0.0}, {-1.0, 0.0, 0.0}, {0.0, 0.0, -1.0}},
1756         {{0.0, 0.0, 1.0}, {-1.0, 0.0, 0.0}, {0.0, -1.0, 0.0}},
1757         {{0.0, 0.0, -1.0}, {-1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}},
1758         {{0.0, 1.0, 0.0}, {-1.0, 0.0, 0.0}, {0.0, 0.0, 1.0}},
1759         {{0.0, -1.0, 0.0}, {0.0, 0.0, 1.0}, {-1.0, 0.0, 0.0}},
1760         {{0.0, 0.0, -1.0}, {0.0, -1.0, 0.0}, {-1.0, 0.0, 0.0}},
1761         {{0.0, 0.0, 1.0}, {0.0, 1.0, 0.0}, {-1.0, 0.0, 0.0}},
1762         {{0.0, 1.0, 0.0}, {0.0, 0.0, -1.0}, {-1.0, 0.0, 0.0}},
1763         {{0.0, -1.0, 0.0}, {0.0, 0.0, -1.0}, {1.0, 0.0, 0.0}},
1764         {{0.0, 0.0, 1.0}, {0.0, -1.0, 0.0}, {1.0, 0.0, 0.0}},
1765         {{0.0, 0.0, -1.0}, {0.0, 1.0, 0.0}, {1.0, 0.0, 0.0}},
1766         {{0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}, {1.0, 0.0, 0.0}},
1767         {{0.0, -1.0, 0.0}, {1.0, 0.0, 0.0}, {0.0, 0.0, 1.0}},
1768         {{0.0, 0.0, -1.0}, {1.0, 0.0, 0.0}, {0.0, -1.0, 0.0}},
1769         {{0.0, 0.0, 1.0}, {1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}},
1770         {{0.0, 1.0, 0.0}, {1.0, 0.0, 0.0}, {0.0, 0.0, -1.0}},
1771         {{1.0, 0.0, 0.0}, {0.0, -1.0, 0.0}, {0.0, 0.0, -1.0}},
1772         {{1.0, 0.0, 0.0}, {0.0, 0.0, 1.0}, {0.0, -1.0, 0.0}},
1773         {{1.0, 0.0, 0.0}, {0.0, 0.0, -1.0}, {0.0, 1.0, 0.0}},
1774 };
1775
1776 static int _axis_convert_lut[23][24] = {
1777         {0x8C8, 0x4D0, 0x2E0, 0xAE8, 0x701, 0x511, 0x119, 0xB29, 0x682, 0x88A,
1778          0x09A, 0x2A2, 0x80B, 0x413, 0x223, 0xA2B, 0x644, 0x454, 0x05C, 0xA6C,
1779          0x745, 0x94D, 0x15D, 0x365},
1780         {0xAC8, 0x8D0, 0x4E0, 0x2E8, 0x741, 0x951, 0x159, 0x369, 0x702, 0xB0A,
1781          0x11A, 0x522, 0xA0B, 0x813, 0x423, 0x22B, 0x684, 0x894, 0x09C, 0x2AC,
1782          0x645, 0xA4D, 0x05D, 0x465},
1783         {0x4C8, 0x2D0, 0xAE0, 0x8E8, 0x681, 0x291, 0x099, 0x8A9, 0x642, 0x44A,
1784          0x05A, 0xA62, 0x40B, 0x213, 0xA23, 0x82B, 0x744, 0x354, 0x15C, 0x96C,
1785          0x705, 0x50D, 0x11D, 0xB25},
1786         {0x2C8, 0xAD0, 0x8E0, 0x4E8, 0x641, 0xA51, 0x059, 0x469, 0x742, 0x34A,
1787          0x15A, 0x962, 0x20B, 0xA13, 0x823, 0x42B, 0x704, 0xB14, 0x11C, 0x52C,
1788          0x685, 0x28D, 0x09D, 0x8A5},
1789         {0x708, 0xB10, 0x120, 0x528, 0x8C1, 0xAD1, 0x2D9, 0x4E9, 0x942, 0x74A,
1790          0x35A, 0x162, 0x64B, 0xA53, 0x063, 0x46B, 0x804, 0xA14, 0x21C, 0x42C,
1791          0x885, 0x68D, 0x29D, 0x0A5},
1792         {0xB08, 0x110, 0x520, 0x728, 0x941, 0x151, 0x359, 0x769, 0x802, 0xA0A,
1793          0x21A, 0x422, 0xA4B, 0x053, 0x463, 0x66B, 0x884, 0x094, 0x29C, 0x6AC,
1794          0x8C5, 0xACD, 0x2DD, 0x4E5},
1795         {0x508, 0x710, 0xB20, 0x128, 0x881, 0x691, 0x299, 0x0A9, 0x8C2, 0x4CA,
1796          0x2DA, 0xAE2, 0x44B, 0x653, 0xA63, 0x06B, 0x944, 0x754, 0x35C, 0x16C,
1797          0x805, 0x40D, 0x21D, 0xA25},
1798         {0x108, 0x510, 0x720, 0xB28, 0x801, 0x411, 0x219, 0xA29, 0x882, 0x08A,
1799          0x29A, 0x6A2, 0x04B, 0x453, 0x663, 0xA6B, 0x8C4, 0x4D4, 0x2DC, 0xAEC,
1800          0x945, 0x14D, 0x35D, 0x765},
1801         {0x748, 0x350, 0x160, 0x968, 0xAC1, 0x2D1, 0x4D9, 0x8E9, 0xA42, 0x64A,
1802          0x45A, 0x062, 0x68B, 0x293, 0x0A3, 0x8AB, 0xA04, 0x214, 0x41C, 0x82C,
1803          0xB05, 0x70D, 0x51D, 0x125},
1804         {0x948, 0x750, 0x360, 0x168, 0xB01, 0x711, 0x519, 0x129, 0xAC2, 0x8CA,
1805          0x4DA, 0x2E2, 0x88B, 0x693, 0x2A3, 0x0AB, 0xA44, 0x654, 0x45C, 0x06C,
1806          0xA05, 0x80D, 0x41D, 0x225},
1807         {0x348, 0x150, 0x960, 0x768, 0xA41, 0x051, 0x459, 0x669, 0xA02, 0x20A,
1808          0x41A, 0x822, 0x28B, 0x093, 0x8A3, 0x6AB, 0xB04, 0x114, 0x51C, 0x72C,
1809          0xAC5, 0x2CD, 0x4DD, 0x8E5},
1810         {0x148, 0x950, 0x760, 0x368, 0xA01, 0x811, 0x419, 0x229, 0xB02, 0x10A,
1811          0x51A, 0x722, 0x08B, 0x893, 0x6A3, 0x2AB, 0xAC4, 0x8D4, 0x4DC, 0x2EC,
1812          0xA45, 0x04D, 0x45D, 0x665},
1813         {0x688, 0x890, 0x0A0, 0x2A8, 0x4C1, 0x8D1, 0xAD9, 0x2E9, 0x502, 0x70A,
1814          0xB1A, 0x122, 0x74B, 0x953, 0x163, 0x36B, 0x404, 0x814, 0xA1C, 0x22C,
1815          0x445, 0x64D, 0xA5D, 0x065},
1816         {0x888, 0x090, 0x2A0, 0x6A8, 0x501, 0x111, 0xB19, 0x729, 0x402, 0x80A,
1817          0xA1A, 0x222, 0x94B, 0x153, 0x363, 0x76B, 0x444, 0x054, 0xA5C, 0x66C,
1818          0x4C5, 0x8CD, 0xADD, 0x2E5},
1819         {0x288, 0x690, 0x8A0, 0x0A8, 0x441, 0x651, 0xA59, 0x069, 0x4C2, 0x2CA,
1820          0xADA, 0x8E2, 0x34B, 0x753, 0x963, 0x16B, 0x504, 0x714, 0xB1C, 0x12C,
1821          0x405, 0x20D, 0xA1D, 0x825},
1822         {0x088, 0x290, 0x6A0, 0x8A8, 0x401, 0x211, 0xA19, 0x829, 0x442, 0x04A,
1823          0xA5A, 0x662, 0x14B, 0x353, 0x763, 0x96B, 0x4C4, 0x2D4, 0xADC, 0x8EC,
1824          0x505, 0x10D, 0xB1D, 0x725},
1825         {0x648, 0x450, 0x060, 0xA68, 0x2C1, 0x4D1, 0x8D9, 0xAE9, 0x282, 0x68A,
1826          0x89A, 0x0A2, 0x70B, 0x513, 0x123, 0xB2B, 0x204, 0x414, 0x81C, 0xA2C,
1827          0x345, 0x74D, 0x95D, 0x165},
1828         {0xA48, 0x650, 0x460, 0x068, 0x341, 0x751, 0x959, 0x169, 0x2C2, 0xACA,
1829          0x8DA, 0x4E2, 0xB0B, 0x713, 0x523, 0x12B, 0x284, 0x694, 0x89C, 0x0AC,
1830          0x205, 0xA0D, 0x81D, 0x425},
1831         {0x448, 0x050, 0xA60, 0x668, 0x281, 0x091, 0x899, 0x6A9, 0x202, 0x40A,
1832          0x81A, 0xA22, 0x50B, 0x113, 0xB23, 0x72B, 0x344, 0x154, 0x95C, 0x76C,
1833          0x2C5, 0x4CD, 0x8DD, 0xAE5},
1834         {0x048, 0xA50, 0x660, 0x468, 0x201, 0xA11, 0x819, 0x429, 0x342, 0x14A,
1835          0x95A, 0x762, 0x10B, 0xB13, 0x723, 0x52B, 0x2C4, 0xAD4, 0x8DC, 0x4EC,
1836          0x285, 0x08D, 0x89D, 0x6A5},
1837         {0x808, 0xA10, 0x220, 0x428, 0x101, 0xB11, 0x719, 0x529, 0x142, 0x94A,
1838          0x75A, 0x362, 0x8CB, 0xAD3, 0x2E3, 0x4EB, 0x044, 0xA54, 0x65C, 0x46C,
1839          0x085, 0x88D, 0x69D, 0x2A5},
1840         {0xA08, 0x210, 0x420, 0x828, 0x141, 0x351, 0x759, 0x969, 0x042, 0xA4A,
1841          0x65A, 0x462, 0xACB, 0x2D3, 0x4E3, 0x8EB, 0x084, 0x294, 0x69C, 0x8AC,
1842          0x105, 0xB0D, 0x71D, 0x525},
1843         {0x408, 0x810, 0xA20, 0x228, 0x081, 0x891, 0x699, 0x2A9, 0x102, 0x50A,
1844          0x71A, 0xB22, 0x4CB, 0x8D3, 0xAE3, 0x2EB, 0x144, 0x954, 0x75C, 0x36C,
1845          0x045, 0x44D, 0x65D, 0xA65},
1846         };
1847
1848 // _axis_convert_num = {'X': 0, 'Y': 1, 'Z': 2, '-X': 3, '-Y': 4, '-Z': 5}
1849
1850 MINLINE int _axis_signed(const int axis)
1851 {
1852         return (axis < 3) ? axis : axis - 3;
1853 }
1854
1855 /*
1856  * Each argument us an axis in ['X', 'Y', 'Z', '-X', '-Y', '-Z']
1857  * where the first 2 are a source and the second 2 are the target.
1858  */
1859 int mat3_from_axis_conversion(int from_forward, int from_up, int to_forward, int to_up,
1860                               float r_mat[3][3])
1861 {
1862         // from functools import reduce
1863         int value;
1864         int i;
1865
1866         if (from_forward == to_forward && from_up == to_up) {
1867                 unit_m3(r_mat);
1868                 return false;
1869         }
1870
1871         if ((_axis_signed(from_forward) == _axis_signed(from_up)) ||
1872             (_axis_signed(to_forward)   == _axis_signed(to_up)))
1873         {
1874                 /* we could assert here! */
1875                 unit_m3(r_mat);
1876                 return false;
1877         }
1878
1879         value = ((from_forward << (0 * 3)) |
1880                  (from_up      << (1 * 3)) |
1881                  (to_forward   << (2 * 3)) |
1882                  (to_up        << (3 * 3)));
1883
1884         for (i = 0; i < (sizeof(_axis_convert_matrix) / sizeof(*_axis_convert_matrix)); i++) {
1885                 int j;
1886                 for (j = 0; j < sizeof(*_axis_convert_lut) / sizeof(*_axis_convert_lut[0]); j++) {
1887                         if (_axis_convert_lut[i][j] == value) {
1888                                 copy_m3_m3(r_mat, _axis_convert_matrix[i]);
1889                                 return true;
1890                         }
1891                 }
1892
1893         }
1894 //      BLI_assert(0);
1895         return false;
1896 }