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