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