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