style cleanup: comments
[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
31
32 #include <assert.h>
33 #include "BLI_math.h"
34
35 /******************************** Quaternions ********************************/
36
37 /* used to test is a quat is not normalized (only used for debug prints) */
38 #ifdef DEBUG
39 #  define QUAT_EPSILON 0.0001
40 #endif
41
42 /* convenience, avoids setting Y axis everywhere */
43 void unit_axis_angle(float axis[3], float *angle)
44 {
45         axis[0] = 0.0f;
46         axis[1] = 1.0f;
47         axis[2] = 0.0f;
48         *angle = 0.0f;
49 }
50
51 void unit_qt(float q[4])
52 {
53         q[0] = 1.0f;
54         q[1] = q[2] = q[3] = 0.0f;
55 }
56
57 void copy_qt_qt(float q1[4], const float q2[4])
58 {
59         q1[0] = q2[0];
60         q1[1] = q2[1];
61         q1[2] = q2[2];
62         q1[3] = q2[3];
63 }
64
65 int is_zero_qt(float *q)
66 {
67         return (q[0] == 0 && q[1] == 0 && q[2] == 0 && q[3] == 0);
68 }
69
70 void mul_qt_qtqt(float q[4], const float q1[4], const float q2[4])
71 {
72         float t0, t1, t2;
73
74         t0 = q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3];
75         t1 = q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2];
76         t2 = q1[0] * q2[2] + q1[2] * q2[0] + q1[3] * q2[1] - q1[1] * q2[3];
77         q[3] = q1[0] * q2[3] + q1[3] * q2[0] + q1[1] * q2[2] - q1[2] * q2[1];
78         q[0] = t0;
79         q[1] = t1;
80         q[2] = t2;
81 }
82
83 /**
84  * \note:
85  * Assumes a unit quaternion?
86  *
87  * in fact not, but you may wan't 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 void mul_qt_v3(const float q[4], float v[3])
101 {
102         float t0, t1, t2;
103
104         t0 = -q[1] * v[0] - q[2] * v[1] - q[3] * v[2];
105         t1 = q[0] * v[0] + q[2] * v[2] - q[3] * v[1];
106         t2 = q[0] * v[1] + q[3] * v[0] - q[1] * v[2];
107         v[2] = q[0] * v[2] + q[1] * v[1] - q[2] * v[0];
108         v[0] = t1;
109         v[1] = t2;
110
111         t1 = t0 * -q[1] + v[0] * q[0] - v[1] * q[3] + v[2] * q[2];
112         t2 = t0 * -q[2] + v[1] * q[0] - v[2] * q[1] + v[0] * q[3];
113         v[2] = t0 * -q[3] + v[2] * q[0] - v[0] * q[2] + v[1] * q[1];
114         v[0] = t1;
115         v[1] = t2;
116 }
117
118 void conjugate_qt_qt(float q1[4], const float q2[4])
119 {
120         q1[0] =  q2[0];
121         q1[1] = -q2[1];
122         q1[2] = -q2[2];
123         q1[3] = -q2[3];
124 }
125
126 void conjugate_qt(float q[4])
127 {
128         q[1] = -q[1];
129         q[2] = -q[2];
130         q[3] = -q[3];
131 }
132
133 float dot_qtqt(const float q1[4], const float q2[4])
134 {
135         return q1[0] * q2[0] + q1[1] * q2[1] + q1[2] * q2[2] + q1[3] * q2[3];
136 }
137
138 void invert_qt(float q[4])
139 {
140         float f = dot_qtqt(q, q);
141
142         if (f == 0.0f)
143                 return;
144
145         conjugate_qt(q);
146         mul_qt_fl(q, 1.0f / f);
147 }
148
149 void invert_qt_qt(float q1[4], const float q2[4])
150 {
151         copy_qt_qt(q1, q2);
152         invert_qt(q1);
153 }
154
155 /* simple mult */
156 void mul_qt_fl(float q[4], const float f)
157 {
158         q[0] *= f;
159         q[1] *= f;
160         q[2] *= f;
161         q[3] *= f;
162 }
163
164 void sub_qt_qtqt(float q[4], const float q1[4], const float q2[4])
165 {
166         float nq2[4];
167
168         nq2[0] = -q2[0];
169         nq2[1] = q2[1];
170         nq2[2] = q2[2];
171         nq2[3] = q2[3];
172
173         mul_qt_qtqt(q, q1, nq2);
174 }
175
176 /* angular mult factor */
177 void mul_fac_qt_fl(float q[4], const float fac)
178 {
179         const float angle = fac * saacos(q[0]); /* quat[0] = cos(0.5 * angle), but now the 0.5 and 2.0 rule out */
180         const float co = cosf(angle);
181         const float si = sinf(angle);
182         q[0] = co;
183         normalize_v3(q + 1);
184         mul_v3_fl(q + 1, si);
185 }
186
187 /* skip error check, currently only needed by mat3_to_quat_is_ok */
188 static void quat_to_mat3_no_error(float m[][3], const float q[4])
189 {
190         double q0, q1, q2, q3, qda, qdb, qdc, qaa, qab, qac, qbb, qbc, qcc;
191
192         q0 = M_SQRT2 * (double)q[0];
193         q1 = M_SQRT2 * (double)q[1];
194         q2 = M_SQRT2 * (double)q[2];
195         q3 = M_SQRT2 * (double)q[3];
196
197         qda = q0 * q1;
198         qdb = q0 * q2;
199         qdc = q0 * q3;
200         qaa = q1 * q1;
201         qab = q1 * q2;
202         qac = q1 * q3;
203         qbb = q2 * q2;
204         qbc = q2 * q3;
205         qcc = q3 * q3;
206
207         m[0][0] = (float)(1.0 - qbb - qcc);
208         m[0][1] = (float)(qdc + qab);
209         m[0][2] = (float)(-qdb + qac);
210
211         m[1][0] = (float)(-qdc + qab);
212         m[1][1] = (float)(1.0 - qaa - qcc);
213         m[1][2] = (float)(qda + qbc);
214
215         m[2][0] = (float)(qdb + qac);
216         m[2][1] = (float)(-qda + qbc);
217         m[2][2] = (float)(1.0 - qaa - qbb);
218 }
219
220 void quat_to_mat3(float m[][3], const float q[4])
221 {
222 #ifdef DEBUG
223         float f;
224         if (!((f = dot_qtqt(q, q)) == 0.0f || (fabsf(f - 1.0f) < (float)QUAT_EPSILON))) {
225                 fprintf(stderr, "Warning! quat_to_mat3() called with non-normalized: size %.8f *** report a bug ***\n", f);
226         }
227 #endif
228
229         quat_to_mat3_no_error(m, q);
230 }
231
232 void quat_to_mat4(float m[][4], const float q[4])
233 {
234         double q0, q1, q2, q3, qda, qdb, qdc, qaa, qab, qac, qbb, qbc, qcc;
235
236 #ifdef DEBUG
237         if (!((q0 = dot_qtqt(q, q)) == 0.0f || (fabsf(q0 - 1.0) < QUAT_EPSILON))) {
238                 fprintf(stderr, "Warning! quat_to_mat4() called with non-normalized: size %.8f *** report a bug ***\n", (float)q0);
239         }
240 #endif
241
242         q0 = M_SQRT2 * (double)q[0];
243         q1 = M_SQRT2 * (double)q[1];
244         q2 = M_SQRT2 * (double)q[2];
245         q3 = M_SQRT2 * (double)q[3];
246
247         qda = q0 * q1;
248         qdb = q0 * q2;
249         qdc = q0 * q3;
250         qaa = q1 * q1;
251         qab = q1 * q2;
252         qac = q1 * q3;
253         qbb = q2 * q2;
254         qbc = q2 * q3;
255         qcc = q3 * q3;
256
257         m[0][0] = (float)(1.0 - qbb - qcc);
258         m[0][1] = (float)(qdc + qab);
259         m[0][2] = (float)(-qdb + qac);
260         m[0][3] = 0.0f;
261
262         m[1][0] = (float)(-qdc + qab);
263         m[1][1] = (float)(1.0 - qaa - qcc);
264         m[1][2] = (float)(qda + qbc);
265         m[1][3] = 0.0f;
266
267         m[2][0] = (float)(qdb + qac);
268         m[2][1] = (float)(-qda + qbc);
269         m[2][2] = (float)(1.0 - qaa - qbb);
270         m[2][3] = 0.0f;
271
272         m[3][0] = m[3][1] = m[3][2] = 0.0f;
273         m[3][3] = 1.0f;
274 }
275
276 void mat3_to_quat(float q[4], float wmat[][3])
277 {
278         double tr, s;
279         float mat[3][3];
280
281         /* work on a copy */
282         copy_m3_m3(mat, wmat);
283         normalize_m3(mat); /* this is needed AND a 'normalize_qt' in the end */
284
285         tr = 0.25 * (double)(1.0f + mat[0][0] + mat[1][1] + mat[2][2]);
286
287         if (tr > (double)FLT_EPSILON) {
288                 s = sqrt(tr);
289                 q[0] = (float)s;
290                 s = 1.0 / (4.0 * s);
291                 q[1] = (float)((mat[1][2] - mat[2][1]) * s);
292                 q[2] = (float)((mat[2][0] - mat[0][2]) * s);
293                 q[3] = (float)((mat[0][1] - mat[1][0]) * s);
294         }
295         else {
296                 if (mat[0][0] > mat[1][1] && mat[0][0] > mat[2][2]) {
297                         s = 2.0f * sqrtf(1.0f + mat[0][0] - mat[1][1] - mat[2][2]);
298                         q[1] = (float)(0.25 * s);
299
300                         s = 1.0 / s;
301                         q[0] = (float)((double)(mat[2][1] - mat[1][2]) * s);
302                         q[2] = (float)((double)(mat[1][0] + mat[0][1]) * s);
303                         q[3] = (float)((double)(mat[2][0] + mat[0][2]) * s);
304                 }
305                 else if (mat[1][1] > mat[2][2]) {
306                         s = 2.0f * sqrtf(1.0f + mat[1][1] - mat[0][0] - mat[2][2]);
307                         q[2] = (float)(0.25 * s);
308
309                         s = 1.0 / s;
310                         q[0] = (float)((double)(mat[2][0] - mat[0][2]) * s);
311                         q[1] = (float)((double)(mat[1][0] + mat[0][1]) * s);
312                         q[3] = (float)((double)(mat[2][1] + mat[1][2]) * s);
313                 }
314                 else {
315                         s = 2.0f * sqrtf(1.0f + mat[2][2] - mat[0][0] - mat[1][1]);
316                         q[3] = (float)(0.25 * s);
317
318                         s = 1.0 / s;
319                         q[0] = (float)((double)(mat[1][0] - mat[0][1]) * s);
320                         q[1] = (float)((double)(mat[2][0] + mat[0][2]) * s);
321                         q[2] = (float)((double)(mat[2][1] + mat[1][2]) * s);
322                 }
323         }
324
325         normalize_qt(q);
326 }
327
328 void mat4_to_quat(float q[4], float m[][4])
329 {
330         float mat[3][3];
331
332         copy_m3_m4(mat, m);
333         mat3_to_quat(q, mat);
334 }
335
336 void mat3_to_quat_is_ok(float q[4], float wmat[3][3])
337 {
338         float mat[3][3], matr[3][3], matn[3][3], q1[4], q2[4], angle, si, co, nor[3];
339
340         /* work on a copy */
341         copy_m3_m3(mat, wmat);
342         normalize_m3(mat);
343
344         /* rotate z-axis of matrix to z-axis */
345
346         nor[0] = mat[2][1]; /* cross product with (0,0,1) */
347         nor[1] = -mat[2][0];
348         nor[2] = 0.0;
349         normalize_v3(nor);
350
351         co = mat[2][2];
352         angle = 0.5f * saacos(co);
353
354         co = cosf(angle);
355         si = sinf(angle);
356         q1[0] = co;
357         q1[1] = -nor[0] * si; /* negative here, but why? */
358         q1[2] = -nor[1] * si;
359         q1[3] = -nor[2] * si;
360
361         /* rotate back x-axis from mat, using inverse q1 */
362         quat_to_mat3_no_error(matr, q1);
363         invert_m3_m3(matn, matr);
364         mul_m3_v3(matn, mat[0]);
365
366         /* and align x-axes */
367         angle = (float)(0.5 * atan2(mat[0][1], mat[0][0]));
368
369         co = cosf(angle);
370         si = sinf(angle);
371         q2[0] = co;
372         q2[1] = 0.0f;
373         q2[2] = 0.0f;
374         q2[3] = si;
375
376         mul_qt_qtqt(q, q1, q2);
377 }
378
379 float normalize_qt(float q[4])
380 {
381         float len;
382
383         len = sqrtf(dot_qtqt(q, q));
384         if (len != 0.0f) {
385                 mul_qt_fl(q, 1.0f / len);
386         }
387         else {
388                 q[1] = 1.0f;
389                 q[0] = q[2] = q[3] = 0.0f;
390         }
391
392         return len;
393 }
394
395 float normalize_qt_qt(float r[4], const float q[4])
396 {
397         copy_qt_qt(r, q);
398         return normalize_qt(r);
399 }
400
401 /* note: expects vectors to be normalized */
402 void rotation_between_vecs_to_quat(float q[4], const float v1[3], const float v2[3])
403 {
404         float axis[3];
405         float angle;
406
407         cross_v3_v3v3(axis, v1, v2);
408
409         angle = angle_normalized_v3v3(v1, v2);
410
411         axis_angle_to_quat(q, axis, angle);
412 }
413
414 void rotation_between_quats_to_quat(float q[4], const float q1[4], const float q2[4])
415 {
416         float tquat[4];
417
418         conjugate_qt_qt(tquat, q1);
419
420         mul_qt_fl(tquat, 1.0f / dot_qtqt(tquat, tquat));
421
422         mul_qt_qtqt(q, tquat, q2);
423 }
424
425 void vec_to_quat(float q[4], const float vec[3], short axis, const short upflag)
426 {
427         float nor[3], tvec[3];
428         float angle, si, co, len;
429
430         assert(axis >= 0 && axis <= 5);
431         assert(upflag >= 0 && upflag <= 2);
432
433         /* first set the quat to unit */
434         unit_qt(q);
435
436         len = len_v3(vec);
437
438         if (UNLIKELY(len == 0.0f)) {
439                 return;
440         }
441
442         /* rotate to axis */
443         if (axis > 2) {
444                 copy_v3_v3(tvec, vec);
445                 axis -= 3;
446         }
447         else {
448                 negate_v3_v3(tvec, vec);
449         }
450
451         /* nasty! I need a good routine for this...
452          * problem is a rotation of an Y axis to the negative Y-axis for example.
453          */
454
455         if (axis == 0) { /* x-axis */
456                 nor[0] =  0.0;
457                 nor[1] = -tvec[2];
458                 nor[2] =  tvec[1];
459
460                 if (fabsf(tvec[1]) + fabsf(tvec[2]) < 0.0001f)
461                         nor[1] = 1.0f;
462
463                 co = tvec[0];
464         }
465         else if (axis == 1) { /* y-axis */
466                 nor[0] =  tvec[2];
467                 nor[1] =  0.0;
468                 nor[2] = -tvec[0];
469
470                 if (fabsf(tvec[0]) + fabsf(tvec[2]) < 0.0001f)
471                         nor[2] = 1.0f;
472
473                 co = tvec[1];
474         }
475         else { /* z-axis */
476                 nor[0] = -tvec[1];
477                 nor[1] =  tvec[0];
478                 nor[2] =  0.0;
479
480                 if (fabsf(tvec[0]) + fabsf(tvec[1]) < 0.0001f)
481                         nor[0] = 1.0f;
482
483                 co = tvec[2];
484         }
485         co /= len;
486
487         normalize_v3(nor);
488
489         angle = 0.5f * saacos(co);
490         si   = sinf(angle);
491         q[0] = cosf(angle);
492         q[1] = nor[0] * si;
493         q[2] = nor[1] * si;
494         q[3] = nor[2] * si;
495
496         if (axis != upflag) {
497                 float mat[3][3];
498                 float q2[4];
499                 const float *fp = mat[2];
500                 quat_to_mat3(mat, q);
501
502                 if (axis == 0) {
503                         if (upflag == 1) angle =  0.5f * atan2f(fp[2], fp[1]);
504                         else             angle = -0.5f * atan2f(fp[1], fp[2]);
505                 }
506                 else if (axis == 1) {
507                         if (upflag == 0) angle = -0.5f * atan2f(fp[2], fp[0]);
508                         else             angle =  0.5f * atan2f(fp[0], fp[2]);
509                 }
510                 else {
511                         if (upflag == 0) angle =  0.5f * atan2f(-fp[1], -fp[0]);
512                         else             angle = -0.5f * atan2f(-fp[0], -fp[1]);
513                 }
514
515                 co = cosf(angle);
516                 si = sinf(angle) / len;
517                 q2[0] = co;
518                 q2[1] = tvec[0] * si;
519                 q2[2] = tvec[1] * si;
520                 q2[3] = tvec[2] * si;
521
522                 mul_qt_qtqt(q, q2, q);
523         }
524 }
525
526 #if 0
527
528 /* A & M Watt, Advanced animation and rendering techniques, 1992 ACM press */
529 void QuatInterpolW(float *result, float quat1[4], float quat2[4], float t)
530 {
531         float omega, cosom, sinom, sc1, sc2;
532
533         cosom = quat1[0] * quat2[0] + quat1[1] * quat2[1] + quat1[2] * quat2[2] + quat1[3] * quat2[3];
534
535         /* rotate around shortest angle */
536         if ((1.0f + cosom) > 0.0001f) {
537
538                 if ((1.0f - cosom) > 0.0001f) {
539                         omega = (float)acos(cosom);
540                         sinom = (float)sin(omega);
541                         sc1 = (float)sin((1.0 - t) * omega) / sinom;
542                         sc2 = (float)sin(t * omega) / sinom;
543                 }
544                 else {
545                         sc1 = 1.0f - t;
546                         sc2 = t;
547                 }
548                 result[0] = sc1 * quat1[0] + sc2 * quat2[0];
549                 result[1] = sc1 * quat1[1] + sc2 * quat2[1];
550                 result[2] = sc1 * quat1[2] + sc2 * quat2[2];
551                 result[3] = sc1 * quat1[3] + sc2 * quat2[3];
552         }
553         else {
554                 result[0] = quat2[3];
555                 result[1] = -quat2[2];
556                 result[2] = quat2[1];
557                 result[3] = -quat2[0];
558
559                 sc1 = (float)sin((1.0 - t) * M_PI_2);
560                 sc2 = (float)sin(t * M_PI_2);
561
562                 result[0] = sc1 * quat1[0] + sc2 * result[0];
563                 result[1] = sc1 * quat1[1] + sc2 * result[1];
564                 result[2] = sc1 * quat1[2] + sc2 * result[2];
565                 result[3] = sc1 * quat1[3] + sc2 * result[3];
566         }
567 }
568 #endif
569
570 void interp_qt_qtqt(float result[4], const float quat1[4], const float quat2[4], const float t)
571 {
572         float quat[4], omega, cosom, sinom, sc1, sc2;
573
574         cosom = quat1[0] * quat2[0] + quat1[1] * quat2[1] + quat1[2] * quat2[2] + quat1[3] * quat2[3];
575
576         /* rotate around shortest angle */
577         if (cosom < 0.0f) {
578                 cosom = -cosom;
579                 quat[0] = -quat1[0];
580                 quat[1] = -quat1[1];
581                 quat[2] = -quat1[2];
582                 quat[3] = -quat1[3];
583         }
584         else {
585                 quat[0] = quat1[0];
586                 quat[1] = quat1[1];
587                 quat[2] = quat1[2];
588                 quat[3] = quat1[3];
589         }
590
591         if ((1.0f - cosom) > 0.0001f) {
592                 omega = (float)acos(cosom);
593                 sinom = (float)sin(omega);
594                 sc1 = (float)sin((1 - t) * omega) / sinom;
595                 sc2 = (float)sin(t * omega) / sinom;
596         }
597         else {
598                 sc1 = 1.0f - t;
599                 sc2 = t;
600         }
601
602         result[0] = sc1 * quat[0] + sc2 * quat2[0];
603         result[1] = sc1 * quat[1] + sc2 * quat2[1];
604         result[2] = sc1 * quat[2] + sc2 * quat2[2];
605         result[3] = sc1 * quat[3] + sc2 * quat2[3];
606 }
607
608 void add_qt_qtqt(float result[4], const float quat1[4], const float quat2[4], const float t)
609 {
610         result[0] = quat1[0] + t * quat2[0];
611         result[1] = quat1[1] + t * quat2[1];
612         result[2] = quat1[2] + t * quat2[2];
613         result[3] = quat1[3] + t * quat2[3];
614 }
615
616 /* same as tri_to_quat() but takes pre-computed normal from the triangle
617  * used for ngons when we know their normal */
618 void tri_to_quat_ex(float quat[4], const float v1[3], const float v2[3], const float v3[3],
619                     const float no_orig[3])
620 {
621         /* imaginary x-axis, y-axis triangle is being rotated */
622         float vec[3], q1[4], q2[4], n[3], si, co, angle, mat[3][3], imat[3][3];
623
624         /* move z-axis to face-normal */
625 #if 0
626         normal_tri_v3(vec, v1, v2, v3);
627 #else
628         copy_v3_v3(vec, no_orig);
629         (void)v3;
630 #endif
631
632         n[0] =  vec[1];
633         n[1] = -vec[0];
634         n[2] =  0.0f;
635         normalize_v3(n);
636
637         if (n[0] == 0.0f && n[1] == 0.0f) {
638                 n[0] = 1.0f;
639         }
640
641         angle = -0.5f * (float)saacos(vec[2]);
642         co = cosf(angle);
643         si = sinf(angle);
644         q1[0] = co;
645         q1[1] = n[0] * si;
646         q1[2] = n[1] * si;
647         q1[3] = 0.0f;
648
649         /* rotate back line v1-v2 */
650         quat_to_mat3(mat, q1);
651         invert_m3_m3(imat, mat);
652         sub_v3_v3v3(vec, v2, v1);
653         mul_m3_v3(imat, vec);
654
655         /* what angle has this line with x-axis? */
656         vec[2] = 0.0f;
657         normalize_v3(vec);
658
659         angle = (float)(0.5 * atan2(vec[1], vec[0]));
660         co = cosf(angle);
661         si = sinf(angle);
662         q2[0] = co;
663         q2[1] = 0.0f;
664         q2[2] = 0.0f;
665         q2[3] = si;
666
667         mul_qt_qtqt(quat, q1, q2);
668 }
669
670 void tri_to_quat(float quat[4], const float v1[3], const float v2[3], const float v3[3])
671 {
672         float vec[3];
673         normal_tri_v3(vec, v1, v2, v3);
674         tri_to_quat_ex(quat, v1, v2, v3, vec);
675 }
676
677 void print_qt(const char *str, const float q[4])
678 {
679         printf("%s: %.3f %.3f %.3f %.3f\n", str, q[0], q[1], q[2], q[3]);
680 }
681
682 /******************************** Axis Angle *********************************/
683
684 /* Axis angle to Quaternions */
685 void axis_angle_to_quat(float q[4], const float axis[3], const float angle)
686 {
687         float nor[3];
688
689         if (LIKELY(normalize_v3_v3(nor, axis) != 0.0f)) {
690                 const float phi = angle / 2.0f;
691                 float si;
692                 si   = sinf(phi);
693                 q[0] = cosf(phi);
694                 q[1] = nor[0] * si;
695                 q[2] = nor[1] * si;
696                 q[3] = nor[2] * si;
697         }
698         else {
699                 unit_qt(q);
700         }
701 }
702
703 /* Quaternions to Axis Angle */
704 void quat_to_axis_angle(float axis[3], float *angle, const float q[4])
705 {
706         float ha, si;
707
708 #ifdef DEBUG
709         if (!((ha = dot_qtqt(q, q)) == 0.0f || (fabsf(ha - 1.0f) < (float)QUAT_EPSILON))) {
710                 fprintf(stderr, "Warning! quat_to_axis_angle() called with non-normalized: size %.8f *** report a bug ***\n", ha);
711         }
712 #endif
713
714         /* calculate angle/2, and sin(angle/2) */
715         ha = acosf(q[0]);
716         si = sinf(ha);
717
718         /* from half-angle to angle */
719         *angle = ha * 2;
720
721         /* prevent division by zero for axis conversion */
722         if (fabsf(si) < 0.0005f)
723                 si = 1.0f;
724
725         axis[0] = q[1] / si;
726         axis[1] = q[2] / si;
727         axis[2] = q[3] / si;
728 }
729
730 /* Axis Angle to Euler Rotation */
731 void axis_angle_to_eulO(float eul[3], const short order, const float axis[3], const float angle)
732 {
733         float q[4];
734
735         /* use quaternions as intermediate representation for now... */
736         axis_angle_to_quat(q, axis, angle);
737         quat_to_eulO(eul, order, q);
738 }
739
740 /* Euler Rotation to Axis Angle */
741 void eulO_to_axis_angle(float axis[3], float *angle, const float eul[3], const short order)
742 {
743         float q[4];
744
745         /* use quaternions as intermediate representation for now... */
746         eulO_to_quat(q, eul, order);
747         quat_to_axis_angle(axis, angle, q);
748 }
749
750 /* axis angle to 3x3 matrix - safer version (normalization of axis performed)
751  *
752  * note: we may want a normalized and non normalized version of this function.
753  */
754 void axis_angle_to_mat3(float mat[3][3], const float axis[3], const float angle)
755 {
756         float nor[3], nsi[3], co, si, ico;
757
758         /* normalize the axis first (to remove unwanted scaling) */
759         if (normalize_v3_v3(nor, axis) == 0.0f) {
760                 unit_m3(mat);
761                 return;
762         }
763
764         /* now convert this to a 3x3 matrix */
765         co = cosf(angle);
766         si = sinf(angle);
767
768         ico = (1.0f - co);
769         nsi[0] = nor[0] * si;
770         nsi[1] = nor[1] * si;
771         nsi[2] = nor[2] * si;
772
773         mat[0][0] = ((nor[0] * nor[0]) * ico) + co;
774         mat[0][1] = ((nor[0] * nor[1]) * ico) + nsi[2];
775         mat[0][2] = ((nor[0] * nor[2]) * ico) - nsi[1];
776         mat[1][0] = ((nor[0] * nor[1]) * ico) - nsi[2];
777         mat[1][1] = ((nor[1] * nor[1]) * ico) + co;
778         mat[1][2] = ((nor[1] * nor[2]) * ico) + nsi[0];
779         mat[2][0] = ((nor[0] * nor[2]) * ico) + nsi[1];
780         mat[2][1] = ((nor[1] * nor[2]) * ico) - nsi[0];
781         mat[2][2] = ((nor[2] * nor[2]) * ico) + co;
782 }
783
784 /* axis angle to 4x4 matrix - safer version (normalization of axis performed) */
785 void axis_angle_to_mat4(float mat[4][4], const float axis[3], const float angle)
786 {
787         float tmat[3][3];
788
789         axis_angle_to_mat3(tmat, axis, angle);
790         unit_m4(mat);
791         copy_m4_m3(mat, tmat);
792 }
793
794 /* 3x3 matrix to axis angle (see Mat4ToVecRot too) */
795 void mat3_to_axis_angle(float axis[3], float *angle, float mat[3][3])
796 {
797         float q[4];
798
799         /* use quaternions as intermediate representation */
800         /* TODO: it would be nicer to go straight there... */
801         mat3_to_quat(q, mat);
802         quat_to_axis_angle(axis, angle, q);
803 }
804
805 /* 4x4 matrix to axis angle (see Mat4ToVecRot too) */
806 void mat4_to_axis_angle(float axis[3], float *angle, float mat[4][4])
807 {
808         float q[4];
809
810         /* use quaternions as intermediate representation */
811         /* TODO: it would be nicer to go straight there... */
812         mat4_to_quat(q, mat);
813         quat_to_axis_angle(axis, angle, q);
814 }
815
816 void single_axis_angle_to_mat3(float mat[3][3], const char axis, const float angle)
817 {
818         const float angle_cos = cosf(angle);
819         const float angle_sin = sinf(angle);
820
821         switch (axis) {
822                 case 'X': /* rotation around X */
823                         mat[0][0] = 1.0f;
824                         mat[0][1] = 0.0f;
825                         mat[0][2] = 0.0f;
826                         mat[1][0] = 0.0f;
827                         mat[1][1] = angle_cos;
828                         mat[1][2] = angle_sin;
829                         mat[2][0] = 0.0f;
830                         mat[2][1] = -angle_sin;
831                         mat[2][2] = angle_cos;
832                         break;
833                 case 'Y': /* rotation around Y */
834                         mat[0][0] = angle_cos;
835                         mat[0][1] = 0.0f;
836                         mat[0][2] = -angle_sin;
837                         mat[1][0] = 0.0f;
838                         mat[1][1] = 1.0f;
839                         mat[1][2] = 0.0f;
840                         mat[2][0] = angle_sin;
841                         mat[2][1] = 0.0f;
842                         mat[2][2] = angle_cos;
843                         break;
844                 case 'Z': /* rotation around Z */
845                         mat[0][0] = angle_cos;
846                         mat[0][1] = angle_sin;
847                         mat[0][2] = 0.0f;
848                         mat[1][0] = -angle_sin;
849                         mat[1][1] = angle_cos;
850                         mat[1][2] = 0.0f;
851                         mat[2][0] = 0.0f;
852                         mat[2][1] = 0.0f;
853                         mat[2][2] = 1.0f;
854                         break;
855                 default:
856                         assert(0);
857         }
858 }
859
860 /****************************** Vector/Rotation ******************************/
861 /* TODO: the following calls should probably be deprecated sometime         */
862
863 /* TODO, replace use of this function with axis_angle_to_mat3() */
864 void vec_rot_to_mat3(float mat[][3], const float vec[3], const float phi)
865 {
866         /* rotation of phi radials around vec */
867         float vx, vx2, vy, vy2, vz, vz2, co, si;
868
869         vx = vec[0];
870         vy = vec[1];
871         vz = vec[2];
872         vx2 = vx * vx;
873         vy2 = vy * vy;
874         vz2 = vz * vz;
875         co = cosf(phi);
876         si = sinf(phi);
877
878         mat[0][0] = vx2 + co * (1.0f - vx2);
879         mat[0][1] = vx * vy * (1.0f - co) + vz * si;
880         mat[0][2] = vz * vx * (1.0f - co) - vy * si;
881         mat[1][0] = vx * vy * (1.0f - co) - vz * si;
882         mat[1][1] = vy2 + co * (1.0f - vy2);
883         mat[1][2] = vy * vz * (1.0f - co) + vx * si;
884         mat[2][0] = vz * vx * (1.0f - co) + vy * si;
885         mat[2][1] = vy * vz * (1.0f - co) - vx * si;
886         mat[2][2] = vz2 + co * (1.0f - vz2);
887 }
888
889 /******************************** XYZ Eulers *********************************/
890
891 /* XYZ order */
892 void eul_to_mat3(float mat[][3], const float eul[3])
893 {
894         double ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
895
896         ci = cos(eul[0]);
897         cj = cos(eul[1]);
898         ch = cos(eul[2]);
899         si = sin(eul[0]);
900         sj = sin(eul[1]);
901         sh = sin(eul[2]);
902         cc = ci * ch;
903         cs = ci * sh;
904         sc = si * ch;
905         ss = si * sh;
906
907         mat[0][0] = (float)(cj * ch);
908         mat[1][0] = (float)(sj * sc - cs);
909         mat[2][0] = (float)(sj * cc + ss);
910         mat[0][1] = (float)(cj * sh);
911         mat[1][1] = (float)(sj * ss + cc);
912         mat[2][1] = (float)(sj * cs - sc);
913         mat[0][2] = (float)-sj;
914         mat[1][2] = (float)(cj * si);
915         mat[2][2] = (float)(cj * ci);
916
917 }
918
919 /* XYZ order */
920 void eul_to_mat4(float mat[][4], const float eul[3])
921 {
922         double ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
923
924         ci = cos(eul[0]);
925         cj = cos(eul[1]);
926         ch = cos(eul[2]);
927         si = sin(eul[0]);
928         sj = sin(eul[1]);
929         sh = sin(eul[2]);
930         cc = ci * ch;
931         cs = ci * sh;
932         sc = si * ch;
933         ss = si * sh;
934
935         mat[0][0] = (float)(cj * ch);
936         mat[1][0] = (float)(sj * sc - cs);
937         mat[2][0] = (float)(sj * cc + ss);
938         mat[0][1] = (float)(cj * sh);
939         mat[1][1] = (float)(sj * ss + cc);
940         mat[2][1] = (float)(sj * cs - sc);
941         mat[0][2] = (float)-sj;
942         mat[1][2] = (float)(cj * si);
943         mat[2][2] = (float)(cj * ci);
944
945
946         mat[3][0] = mat[3][1] = mat[3][2] = mat[0][3] = mat[1][3] = mat[2][3] = 0.0f;
947         mat[3][3] = 1.0f;
948 }
949
950 /* returns two euler calculation methods, so we can pick the best */
951
952 /* XYZ order */
953 static void mat3_to_eul2(float tmat[][3], float eul1[3], float eul2[3])
954 {
955         float cy, quat[4], mat[3][3];
956
957         mat3_to_quat(quat, tmat);
958         quat_to_mat3(mat, quat);
959         copy_m3_m3(mat, tmat);
960         normalize_m3(mat);
961
962         cy = (float)sqrt(mat[0][0] * mat[0][0] + mat[0][1] * mat[0][1]);
963
964         if (cy > 16.0f * FLT_EPSILON) {
965
966                 eul1[0] = (float)atan2(mat[1][2], mat[2][2]);
967                 eul1[1] = (float)atan2(-mat[0][2], cy);
968                 eul1[2] = (float)atan2(mat[0][1], mat[0][0]);
969
970                 eul2[0] = (float)atan2(-mat[1][2], -mat[2][2]);
971                 eul2[1] = (float)atan2(-mat[0][2], -cy);
972                 eul2[2] = (float)atan2(-mat[0][1], -mat[0][0]);
973
974         }
975         else {
976                 eul1[0] = (float)atan2(-mat[2][1], mat[1][1]);
977                 eul1[1] = (float)atan2(-mat[0][2], cy);
978                 eul1[2] = 0.0f;
979
980                 copy_v3_v3(eul2, eul1);
981         }
982 }
983
984 /* XYZ order */
985 void mat3_to_eul(float *eul, float tmat[][3])
986 {
987         float eul1[3], eul2[3];
988
989         mat3_to_eul2(tmat, eul1, eul2);
990
991         /* return best, which is just the one with lowest values it in */
992         if (fabsf(eul1[0]) + fabsf(eul1[1]) + fabsf(eul1[2]) > fabsf(eul2[0]) + fabsf(eul2[1]) + fabsf(eul2[2])) {
993                 copy_v3_v3(eul, eul2);
994         }
995         else {
996                 copy_v3_v3(eul, eul1);
997         }
998 }
999
1000 /* XYZ order */
1001 void mat4_to_eul(float *eul, float tmat[][4])
1002 {
1003         float tempMat[3][3];
1004
1005         copy_m3_m4(tempMat, tmat);
1006         normalize_m3(tempMat);
1007         mat3_to_eul(eul, tempMat);
1008 }
1009
1010 /* XYZ order */
1011 void quat_to_eul(float *eul, const float quat[4])
1012 {
1013         float mat[3][3];
1014
1015         quat_to_mat3(mat, quat);
1016         mat3_to_eul(eul, mat);
1017 }
1018
1019 /* XYZ order */
1020 void eul_to_quat(float *quat, const float eul[3])
1021 {
1022         float ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
1023
1024         ti = eul[0] * 0.5f;
1025         tj = eul[1] * 0.5f;
1026         th = eul[2] * 0.5f;
1027         ci = cosf(ti);
1028         cj = cosf(tj);
1029         ch = cosf(th);
1030         si = sinf(ti);
1031         sj = sinf(tj);
1032         sh = sinf(th);
1033         cc = ci * ch;
1034         cs = ci * sh;
1035         sc = si * ch;
1036         ss = si * sh;
1037
1038         quat[0] = cj * cc + sj * ss;
1039         quat[1] = cj * sc - sj * cs;
1040         quat[2] = cj * ss + sj * cc;
1041         quat[3] = cj * cs - sj * sc;
1042 }
1043
1044 /* XYZ order */
1045 void rotate_eul(float *beul, const char axis, const float ang)
1046 {
1047         float eul[3], mat1[3][3], mat2[3][3], totmat[3][3];
1048
1049         assert(axis >= 'X' && axis <= 'Z');
1050
1051         eul[0] = eul[1] = eul[2] = 0.0f;
1052         if (axis == 'X') eul[0] = ang;
1053         else if (axis == 'Y') eul[1] = ang;
1054         else eul[2] = ang;
1055
1056         eul_to_mat3(mat1, eul);
1057         eul_to_mat3(mat2, beul);
1058
1059         mul_m3_m3m3(totmat, mat2, mat1);
1060
1061         mat3_to_eul(beul, totmat);
1062
1063 }
1064
1065 /* order independent! */
1066 void compatible_eul(float eul[3], const float oldrot[3])
1067 {
1068         /* we could use M_PI as pi_thresh: which is correct but 5.1 gives better results.
1069          * Checked with baking actions to fcurves - campbell */
1070         const float pi_thresh = (5.1f);
1071         const float pi_x2     = (2.0f * (float)M_PI);
1072
1073         float deul[3];
1074         unsigned int i;
1075
1076         /* correct differences of about 360 degrees first */
1077         for (i = 0; i < 3; i++) {
1078                 deul[i] = eul[i] - oldrot[i];
1079                 if (deul[i] > pi_thresh) {
1080                         eul[i] -= floorf(( deul[i] / pi_x2) + 0.5) * pi_x2;
1081                         deul[i] = eul[i] - oldrot[i];
1082                 }
1083                 else if (deul[i] < -pi_thresh) {
1084                         eul[i] += floorf((-deul[i] / pi_x2) + 0.5) * pi_x2;
1085                         deul[i] = eul[i] - oldrot[i];
1086                 }
1087         }
1088
1089         /* is 1 of the axis rotations larger than 180 degrees and the other small? NO ELSE IF!! */
1090         if (fabsf(deul[0]) > 3.2f && fabsf(deul[1]) < 1.6f && fabsf(deul[2]) < 1.6f) {
1091                 if (deul[0] > 0.0f) eul[0] -= pi_x2;
1092                 else                eul[0] += pi_x2;
1093         }
1094         if (fabsf(deul[1]) > 3.2f && fabsf(deul[2]) < 1.6f && fabsf(deul[0]) < 1.6f) {
1095                 if (deul[1] > 0.0f) eul[1] -= pi_x2;
1096                 else                eul[1] += pi_x2;
1097         }
1098         if (fabsf(deul[2]) > 3.2f && fabsf(deul[0]) < 1.6f && fabsf(deul[1]) < 1.6f) {
1099                 if (deul[2] > 0.0f) eul[2] -= pi_x2;
1100                 else                eul[2] += pi_x2;
1101         }
1102
1103 #undef PI_THRESH
1104 #undef PI_2F
1105 }
1106
1107 /* uses 2 methods to retrieve eulers, and picks the closest */
1108
1109 /* XYZ order */
1110 void mat3_to_compatible_eul(float eul[3], const float oldrot[3], float mat[][3])
1111 {
1112         float eul1[3], eul2[3];
1113         float d1, d2;
1114
1115         mat3_to_eul2(mat, eul1, eul2);
1116
1117         compatible_eul(eul1, oldrot);
1118         compatible_eul(eul2, oldrot);
1119
1120         d1 = fabsf(eul1[0] - oldrot[0]) + fabsf(eul1[1] - oldrot[1]) + fabsf(eul1[2] - oldrot[2]);
1121         d2 = fabsf(eul2[0] - oldrot[0]) + fabsf(eul2[1] - oldrot[1]) + fabsf(eul2[2] - oldrot[2]);
1122
1123         /* return best, which is just the one with lowest difference */
1124         if (d1 > d2) {
1125                 copy_v3_v3(eul, eul2);
1126         }
1127         else {
1128                 copy_v3_v3(eul, eul1);
1129         }
1130
1131 }
1132
1133 /************************** Arbitrary Order Eulers ***************************/
1134
1135 /* Euler Rotation Order Code:
1136  * was adapted from
1137  *      ANSI C code from the article
1138  *      "Euler Angle Conversion"
1139  *      by Ken Shoemake, shoemake@graphics.cis.upenn.edu
1140  *      in "Graphics Gems IV", Academic Press, 1994
1141  * for use in Blender
1142  */
1143
1144 /* Type for rotation order info - see wiki for derivation details */
1145 typedef struct RotOrderInfo {
1146         short axis[3];
1147         short parity; /* parity of axis permutation (even=0, odd=1) - 'n' in original code */
1148 } RotOrderInfo;
1149
1150 /* Array of info for Rotation Order calculations
1151  * WARNING: must be kept in same order as eEulerRotationOrders
1152  */
1153 static const RotOrderInfo rotOrders[] = {
1154         /* i, j, k, n */
1155         {{0, 1, 2}, 0}, /* XYZ */
1156         {{0, 2, 1}, 1}, /* XZY */
1157         {{1, 0, 2}, 1}, /* YXZ */
1158         {{1, 2, 0}, 0}, /* YZX */
1159         {{2, 0, 1}, 0}, /* ZXY */
1160         {{2, 1, 0}, 1}  /* ZYX */
1161 };
1162
1163 /* Get relevant pointer to rotation order set from the array
1164  * NOTE: since we start at 1 for the values, but arrays index from 0,
1165  *               there is -1 factor involved in this process...
1166  */
1167 #define GET_ROTATIONORDER_INFO(order) (assert(order >= 0 && order <= 6), (order < 1) ? &rotOrders[0] : &rotOrders[(order) - 1])
1168
1169 /* Construct quaternion from Euler angles (in radians). */
1170 void eulO_to_quat(float q[4], const float e[3], const short order)
1171 {
1172         const RotOrderInfo *R = GET_ROTATIONORDER_INFO(order);
1173         short i = R->axis[0], j = R->axis[1], k = R->axis[2];
1174         double ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
1175         double a[3];
1176
1177         ti = e[i] * 0.5f;
1178         tj = e[j] * (R->parity ? -0.5f : 0.5f);
1179         th = e[k] * 0.5f;
1180
1181         ci = cos(ti);
1182         cj = cos(tj);
1183         ch = cos(th);
1184         si = sin(ti);
1185         sj = sin(tj);
1186         sh = sin(th);
1187
1188         cc = ci * ch;
1189         cs = ci * sh;
1190         sc = si * ch;
1191         ss = si * sh;
1192
1193         a[i] = cj * sc - sj * cs;
1194         a[j] = cj * ss + sj * cc;
1195         a[k] = cj * cs - sj * sc;
1196
1197         q[0] = cj * cc + sj * ss;
1198         q[1] = a[0];
1199         q[2] = a[1];
1200         q[3] = a[2];
1201
1202         if (R->parity) q[j + 1] = -q[j + 1];
1203 }
1204
1205 /* Convert quaternion to Euler angles (in radians). */
1206 void quat_to_eulO(float e[3], short const order, const float q[4])
1207 {
1208         float M[3][3];
1209
1210         quat_to_mat3(M, q);
1211         mat3_to_eulO(e, order, M);
1212 }
1213
1214 /* Construct 3x3 matrix from Euler angles (in radians). */
1215 void eulO_to_mat3(float M[3][3], const float e[3], const short order)
1216 {
1217         const RotOrderInfo *R = GET_ROTATIONORDER_INFO(order);
1218         short i = R->axis[0], j = R->axis[1], k = R->axis[2];
1219         double ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
1220
1221         if (R->parity) {
1222                 ti = -e[i];
1223                 tj = -e[j];
1224                 th = -e[k];
1225         }
1226         else {
1227                 ti = e[i];
1228                 tj = e[j];
1229                 th = e[k];
1230         }
1231
1232         ci = cos(ti);
1233         cj = cos(tj);
1234         ch = cos(th);
1235         si = sin(ti);
1236         sj = sin(tj);
1237         sh = sin(th);
1238
1239         cc = ci * ch;
1240         cs = ci * sh;
1241         sc = si * ch;
1242         ss = si * sh;
1243
1244         M[i][i] = cj * ch;
1245         M[j][i] = sj * sc - cs;
1246         M[k][i] = sj * cc + ss;
1247         M[i][j] = cj * sh;
1248         M[j][j] = sj * ss + cc;
1249         M[k][j] = sj * cs - sc;
1250         M[i][k] = -sj;
1251         M[j][k] = cj * si;
1252         M[k][k] = cj * ci;
1253 }
1254
1255 /* returns two euler calculation methods, so we can pick the best */
1256 static void mat3_to_eulo2(float M[3][3], float *e1, float *e2, const short order)
1257 {
1258         const RotOrderInfo *R = GET_ROTATIONORDER_INFO(order);
1259         short i = R->axis[0], j = R->axis[1], k = R->axis[2];
1260         float m[3][3];
1261         double cy;
1262
1263         /* process the matrix first */
1264         copy_m3_m3(m, M);
1265         normalize_m3(m);
1266
1267         cy = sqrt(m[i][i] * m[i][i] + m[i][j] * m[i][j]);
1268
1269         if (cy > 16.0 * (double)FLT_EPSILON) {
1270                 e1[i] = atan2(m[j][k], m[k][k]);
1271                 e1[j] = atan2(-m[i][k], cy);
1272                 e1[k] = atan2(m[i][j], m[i][i]);
1273
1274                 e2[i] = atan2(-m[j][k], -m[k][k]);
1275                 e2[j] = atan2(-m[i][k], -cy);
1276                 e2[k] = atan2(-m[i][j], -m[i][i]);
1277         }
1278         else {
1279                 e1[i] = atan2(-m[k][j], m[j][j]);
1280                 e1[j] = atan2(-m[i][k], cy);
1281                 e1[k] = 0;
1282
1283                 copy_v3_v3(e2, e1);
1284         }
1285
1286         if (R->parity) {
1287                 e1[0] = -e1[0];
1288                 e1[1] = -e1[1];
1289                 e1[2] = -e1[2];
1290
1291                 e2[0] = -e2[0];
1292                 e2[1] = -e2[1];
1293                 e2[2] = -e2[2];
1294         }
1295 }
1296
1297 /* Construct 4x4 matrix from Euler angles (in radians). */
1298 void eulO_to_mat4(float M[4][4], const float e[3], const short order)
1299 {
1300         float m[3][3];
1301
1302         /* for now, we'll just do this the slow way (i.e. copying matrices) */
1303         normalize_m3(m);
1304         eulO_to_mat3(m, e, order);
1305         copy_m4_m3(M, m);
1306 }
1307
1308 /* Convert 3x3 matrix to Euler angles (in radians). */
1309 void mat3_to_eulO(float eul[3], const short order, float M[3][3])
1310 {
1311         float eul1[3], eul2[3];
1312
1313         mat3_to_eulo2(M, eul1, eul2, order);
1314
1315         /* return best, which is just the one with lowest values it in */
1316         if (fabsf(eul1[0]) + fabsf(eul1[1]) + fabsf(eul1[2]) > fabsf(eul2[0]) + fabsf(eul2[1]) + fabsf(eul2[2])) {
1317                 copy_v3_v3(eul, eul2);
1318         }
1319         else {
1320                 copy_v3_v3(eul, eul1);
1321         }
1322 }
1323
1324 /* Convert 4x4 matrix to Euler angles (in radians). */
1325 void mat4_to_eulO(float e[3], const short order, float M[4][4])
1326 {
1327         float m[3][3];
1328
1329         /* for now, we'll just do this the slow way (i.e. copying matrices) */
1330         copy_m3_m4(m, M);
1331         normalize_m3(m);
1332         mat3_to_eulO(e, order, m);
1333 }
1334
1335 /* uses 2 methods to retrieve eulers, and picks the closest */
1336 void mat3_to_compatible_eulO(float eul[3], float oldrot[3], const short order, float mat[3][3])
1337 {
1338         float eul1[3], eul2[3];
1339         float d1, d2;
1340
1341         mat3_to_eulo2(mat, eul1, eul2, order);
1342
1343         compatible_eul(eul1, oldrot);
1344         compatible_eul(eul2, oldrot);
1345
1346         d1 = fabsf(eul1[0] - oldrot[0]) + fabsf(eul1[1] - oldrot[1]) + fabsf(eul1[2] - oldrot[2]);
1347         d2 = fabsf(eul2[0] - oldrot[0]) + fabsf(eul2[1] - oldrot[1]) + fabsf(eul2[2] - oldrot[2]);
1348
1349         /* return best, which is just the one with lowest difference */
1350         if (d1 > d2)
1351                 copy_v3_v3(eul, eul2);
1352         else
1353                 copy_v3_v3(eul, eul1);
1354 }
1355
1356 void mat4_to_compatible_eulO(float eul[3], float oldrot[3], const short order, float M[4][4])
1357 {
1358         float m[3][3];
1359
1360         /* for now, we'll just do this the slow way (i.e. copying matrices) */
1361         copy_m3_m4(m, M);
1362         normalize_m3(m);
1363         mat3_to_compatible_eulO(eul, oldrot, order, m);
1364 }
1365 /* rotate the given euler by the given angle on the specified axis */
1366 /* NOTE: is this safe to do with different axis orders? */
1367
1368 void rotate_eulO(float beul[3], const short order, char axis, float ang)
1369 {
1370         float eul[3], mat1[3][3], mat2[3][3], totmat[3][3];
1371
1372         assert(axis >= 'X' && axis <= 'Z');
1373
1374         eul[0] = eul[1] = eul[2] = 0.0f;
1375         if (axis == 'X')
1376                 eul[0] = ang;
1377         else if (axis == 'Y')
1378                 eul[1] = ang;
1379         else
1380                 eul[2] = ang;
1381
1382         eulO_to_mat3(mat1, eul, order);
1383         eulO_to_mat3(mat2, beul, order);
1384
1385         mul_m3_m3m3(totmat, mat2, mat1);
1386
1387         mat3_to_eulO(beul, order, totmat);
1388 }
1389
1390 /* the matrix is written to as 3 axis vectors */
1391 void eulO_to_gimbal_axis(float gmat[][3], const float eul[3], const short order)
1392 {
1393         const RotOrderInfo *R = GET_ROTATIONORDER_INFO(order);
1394
1395         float mat[3][3];
1396         float teul[3];
1397
1398         /* first axis is local */
1399         eulO_to_mat3(mat, eul, order);
1400         copy_v3_v3(gmat[R->axis[0]], mat[R->axis[0]]);
1401
1402         /* second axis is local minus first rotation */
1403         copy_v3_v3(teul, eul);
1404         teul[R->axis[0]] = 0;
1405         eulO_to_mat3(mat, teul, order);
1406         copy_v3_v3(gmat[R->axis[1]], mat[R->axis[1]]);
1407
1408
1409         /* Last axis is global */
1410         gmat[R->axis[2]][0] = 0;
1411         gmat[R->axis[2]][1] = 0;
1412         gmat[R->axis[2]][2] = 0;
1413         gmat[R->axis[2]][R->axis[2]] = 1;
1414 }
1415
1416 /******************************* Dual Quaternions ****************************/
1417
1418 /**
1419  * Conversion routines between (regular quaternion, translation) and
1420  * dual quaternion.
1421  *
1422  * Version 1.0.0, February 7th, 2007
1423  *
1424  * Copyright (C) 2006-2007 University of Dublin, Trinity College, All Rights
1425  * Reserved
1426  *
1427  * This software is provided 'as-is', without any express or implied
1428  * warranty.  In no event will the author(s) be held liable for any damages
1429  * arising from the use of this software.
1430  *
1431  * Permission is granted to anyone to use this software for any purpose,
1432  * including commercial applications, and to alter it and redistribute it
1433  * freely, subject to the following restrictions:
1434  *
1435  * 1. The origin of this software must not be misrepresented; you must not
1436  *    claim that you wrote the original software. If you use this software
1437  *    in a product, an acknowledgment in the product documentation would be
1438  *    appreciated but is not required.
1439  * 2. Altered source versions must be plainly marked as such, and must not be
1440  *    misrepresented as being the original software.
1441  * 3. This notice may not be removed or altered from any source distribution.
1442  *
1443  * \author Ladislav Kavan, kavanl@cs.tcd.ie
1444  *
1445  * Changes for Blender:
1446  * - renaming, style changes and optimization's
1447  * - added support for scaling
1448  */
1449
1450 void mat4_to_dquat(DualQuat *dq, float basemat[][4], float mat[][4])
1451 {
1452         float *t, *q, dscale[3], scale[3], basequat[4];
1453         float baseRS[4][4], baseinv[4][4], baseR[4][4], baseRinv[4][4];
1454         float R[4][4], S[4][4];
1455
1456         /* split scaling and rotation, there is probably a faster way to do
1457          * this, it's done like this now to correctly get negative scaling */
1458         mult_m4_m4m4(baseRS, mat, basemat);
1459         mat4_to_size(scale, baseRS);
1460
1461         dscale[0] = scale[0] - 1.0f;
1462         dscale[1] = scale[1] - 1.0f;
1463         dscale[2] = scale[2] - 1.0f;
1464
1465         if ((determinant_m4(mat) < 0.0f) || len_v3(dscale) > 1e-4f) {
1466                 /* extract R and S  */
1467                 float tmp[4][4];
1468
1469                 /* extra orthogonalize, to avoid flipping with stretched bones */
1470                 copy_m4_m4(tmp, baseRS);
1471                 orthogonalize_m4(tmp, 1);
1472                 mat4_to_quat(basequat, tmp);
1473
1474                 quat_to_mat4(baseR, basequat);
1475                 copy_v3_v3(baseR[3], baseRS[3]);
1476
1477                 invert_m4_m4(baseinv, basemat);
1478                 mult_m4_m4m4(R, baseR, baseinv);
1479
1480                 invert_m4_m4(baseRinv, baseR);
1481                 mult_m4_m4m4(S, baseRinv, baseRS);
1482
1483                 /* set scaling part */
1484                 mul_serie_m4(dq->scale, basemat, S, baseinv, NULL, NULL, NULL, NULL, NULL);
1485                 dq->scale_weight = 1.0f;
1486         }
1487         else {
1488                 /* matrix does not contain scaling */
1489                 copy_m4_m4(R, mat);
1490                 dq->scale_weight = 0.0f;
1491         }
1492
1493         /* non-dual part */
1494         mat4_to_quat(dq->quat, R);
1495
1496         /* dual part */
1497         t = R[3];
1498         q = dq->quat;
1499         dq->trans[0] = -0.5f * ( t[0] * q[1] + t[1] * q[2] + t[2] * q[3]);
1500         dq->trans[1] =  0.5f * ( t[0] * q[0] + t[1] * q[3] - t[2] * q[2]);
1501         dq->trans[2] =  0.5f * (-t[0] * q[3] + t[1] * q[0] + t[2] * q[1]);
1502         dq->trans[3] =  0.5f * ( t[0] * q[2] - t[1] * q[1] + t[2] * q[0]);
1503 }
1504
1505 void dquat_to_mat4(float mat[][4], DualQuat *dq)
1506 {
1507         float len, *t, q0[4];
1508
1509         /* regular quaternion */
1510         copy_qt_qt(q0, dq->quat);
1511
1512         /* normalize */
1513         len = sqrtf(dot_qtqt(q0, q0));
1514         if (len != 0.0f)
1515                 mul_qt_fl(q0, 1.0f / len);
1516
1517         /* rotation */
1518         quat_to_mat4(mat, q0);
1519
1520         /* translation */
1521         t = dq->trans;
1522         mat[3][0] = 2.0f * (-t[0] * q0[1] + t[1] * q0[0] - t[2] * q0[3] + t[3] * q0[2]);
1523         mat[3][1] = 2.0f * (-t[0] * q0[2] + t[1] * q0[3] + t[2] * q0[0] - t[3] * q0[1]);
1524         mat[3][2] = 2.0f * (-t[0] * q0[3] - t[1] * q0[2] + t[2] * q0[1] + t[3] * q0[0]);
1525
1526         /* note: this does not handle scaling */
1527 }
1528
1529 void add_weighted_dq_dq(DualQuat *dqsum, DualQuat *dq, float weight)
1530 {
1531         int flipped = 0;
1532
1533         /* make sure we interpolate quats in the right direction */
1534         if (dot_qtqt(dq->quat, dqsum->quat) < 0) {
1535                 flipped = 1;
1536                 weight = -weight;
1537         }
1538
1539         /* interpolate rotation and translation */
1540         dqsum->quat[0] += weight * dq->quat[0];
1541         dqsum->quat[1] += weight * dq->quat[1];
1542         dqsum->quat[2] += weight * dq->quat[2];
1543         dqsum->quat[3] += weight * dq->quat[3];
1544
1545         dqsum->trans[0] += weight * dq->trans[0];
1546         dqsum->trans[1] += weight * dq->trans[1];
1547         dqsum->trans[2] += weight * dq->trans[2];
1548         dqsum->trans[3] += weight * dq->trans[3];
1549
1550         /* interpolate scale - but only if needed */
1551         if (dq->scale_weight) {
1552                 float wmat[4][4];
1553
1554                 if (flipped) /* we don't want negative weights for scaling */
1555                         weight = -weight;
1556
1557                 copy_m4_m4(wmat, dq->scale);
1558                 mul_m4_fl(wmat, weight);
1559                 add_m4_m4m4(dqsum->scale, dqsum->scale, wmat);
1560                 dqsum->scale_weight += weight;
1561         }
1562 }
1563
1564 void normalize_dq(DualQuat *dq, float totweight)
1565 {
1566         float scale = 1.0f / totweight;
1567
1568         mul_qt_fl(dq->quat, scale);
1569         mul_qt_fl(dq->trans, scale);
1570
1571         if (dq->scale_weight) {
1572                 float addweight = totweight - dq->scale_weight;
1573
1574                 if (addweight) {
1575                         dq->scale[0][0] += addweight;
1576                         dq->scale[1][1] += addweight;
1577                         dq->scale[2][2] += addweight;
1578                         dq->scale[3][3] += addweight;
1579                 }
1580
1581                 mul_m4_fl(dq->scale, scale);
1582                 dq->scale_weight = 1.0f;
1583         }
1584 }
1585
1586 void mul_v3m3_dq(float co[3], float mat[][3], DualQuat *dq)
1587 {
1588         float M[3][3], t[3], scalemat[3][3], len2;
1589         float w = dq->quat[0], x = dq->quat[1], y = dq->quat[2], z = dq->quat[3];
1590         float t0 = dq->trans[0], t1 = dq->trans[1], t2 = dq->trans[2], t3 = dq->trans[3];
1591
1592         /* rotation matrix */
1593         M[0][0] = w * w + x * x - y * y - z * z;
1594         M[1][0] = 2 * (x * y - w * z);
1595         M[2][0] = 2 * (x * z + w * y);
1596
1597         M[0][1] = 2 * (x * y + w * z);
1598         M[1][1] = w * w + y * y - x * x - z * z;
1599         M[2][1] = 2 * (y * z - w * x);
1600
1601         M[0][2] = 2 * (x * z - w * y);
1602         M[1][2] = 2 * (y * z + w * x);
1603         M[2][2] = w * w + z * z - x * x - y * y;
1604
1605         len2 = dot_qtqt(dq->quat, dq->quat);
1606         if (len2 > 0.0f)
1607                 len2 = 1.0f / len2;
1608
1609         /* translation */
1610         t[0] = 2 * (-t0 * x + w * t1 - t2 * z + y * t3);
1611         t[1] = 2 * (-t0 * y + t1 * z - x * t3 + w * t2);
1612         t[2] = 2 * (-t0 * z + x * t2 + w * t3 - t1 * y);
1613
1614         /* apply scaling */
1615         if (dq->scale_weight)
1616                 mul_m4_v3(dq->scale, co);
1617
1618         /* apply rotation and translation */
1619         mul_m3_v3(M, co);
1620         co[0] = (co[0] + t[0]) * len2;
1621         co[1] = (co[1] + t[1]) * len2;
1622         co[2] = (co[2] + t[2]) * len2;
1623
1624         /* compute crazyspace correction mat */
1625         if (mat) {
1626                 if (dq->scale_weight) {
1627                         copy_m3_m4(scalemat, dq->scale);
1628                         mul_m3_m3m3(mat, M, scalemat);
1629                 }
1630                 else
1631                         copy_m3_m3(mat, M);
1632                 mul_m3_fl(mat, len2);
1633         }
1634 }
1635
1636 void copy_dq_dq(DualQuat *dq1, DualQuat *dq2)
1637 {
1638         memcpy(dq1, dq2, sizeof(DualQuat));
1639 }
1640
1641 /* axis matches eTrackToAxis_Modes */
1642 void quat_apply_track(float quat[4], short axis, short upflag)
1643 {
1644         /* rotations are hard coded to match vec_to_quat */
1645         const float quat_track[][4] = {
1646                 {M_SQRT1_2, 0.0, -M_SQRT1_2, 0.0}, /* pos-y90 */
1647                 {0.5, 0.5, 0.5, 0.5}, /* Quaternion((1,0,0), radians(90)) * Quaternion((0,1,0), radians(90)) */
1648                 {M_SQRT1_2, 0.0, 0.0, M_SQRT1_2}, /* pos-z90 */
1649                 {M_SQRT1_2, 0.0, M_SQRT1_2, 0.0}, /* neg-y90 */
1650                 {0.5, -0.5, -0.5, 0.5}, /* Quaternion((1,0,0), radians(-90)) * Quaternion((0,1,0), radians(-90)) */
1651                 {0.0, M_SQRT1_2, M_SQRT1_2, 0.0} /* no rotation */
1652         };
1653
1654         assert(axis >= 0 && axis <= 5);
1655         assert(upflag >= 0 && upflag <= 2);
1656
1657         mul_qt_qtqt(quat, quat, quat_track[axis]);
1658
1659         if (axis > 2)
1660                 axis = axis - 3;
1661
1662         /* there are 2 possible up-axis for each axis used, the 'quat_track' applies so the first
1663          * up axis is used X->Y, Y->X, Z->X, if this first up axis isn used then rotate 90d
1664          * the strange bit shift below just find the low axis {X:Y, Y:X, Z:X} */
1665         if (upflag != (2 - axis) >> 1) {
1666                 float q[4] = {M_SQRT1_2, 0.0, 0.0, 0.0}; /* assign 90d rotation axis */
1667                 q[axis + 1] = ((axis == 1)) ? M_SQRT1_2 : -M_SQRT1_2; /* flip non Y axis */
1668                 mul_qt_qtqt(quat, quat, q);
1669         }
1670 }
1671
1672 void vec_apply_track(float vec[3], short axis)
1673 {
1674         float tvec[3];
1675
1676         assert(axis >= 0 && axis <= 5);
1677
1678         copy_v3_v3(tvec, vec);
1679
1680         switch (axis) {
1681                 case 0: /* pos-x */
1682                         /* vec[0]=  0.0; */
1683                         vec[1] = tvec[2];
1684                         vec[2] = -tvec[1];
1685                         break;
1686                 case 1: /* pos-y */
1687                         /* vec[0]= tvec[0]; */
1688                         /* vec[1]=  0.0; */
1689                         /* vec[2]= tvec[2]; */
1690                         break;
1691                 case 2: /* pos-z */
1692                         /* vec[0]= tvec[0]; */
1693                         /* vec[1]= tvec[1]; */
1694                         /* vec[2]=  0.0; */
1695                         break;
1696                 case 3: /* neg-x */
1697                         /* vec[0]=  0.0; */
1698                         vec[1] = tvec[2];
1699                         vec[2] = -tvec[1];
1700                         break;
1701                 case 4: /* neg-y */
1702                         vec[0] = -tvec[2];
1703                         /* vec[1]=  0.0; */
1704                         vec[2] = tvec[0];
1705                         break;
1706                 case 5: /* neg-z */
1707                         vec[0] = -tvec[0];
1708                         vec[1] = -tvec[1];
1709                         /* vec[2]=  0.0; */
1710                         break;
1711         }
1712 }
1713
1714 /* lens/angle conversion (radians) */
1715 float focallength_to_fov(float focal_length, float sensor)
1716 {
1717         return 2.0f * atanf((sensor / 2.0f) / focal_length);
1718 }
1719
1720 float fov_to_focallength(float hfov, float sensor)
1721 {
1722         return (sensor / 2.0f) / tanf(hfov * 0.5f);
1723 }
1724
1725 /* 'mod_inline(-3,4)= 1', 'fmod(-3,4)= -3' */
1726 static float mod_inline(float a, float b)
1727 {
1728         return a - (b * floorf(a / b));
1729 }
1730
1731 float angle_wrap_rad(float angle)
1732 {
1733         return mod_inline(angle + (float)M_PI, (float)M_PI * 2.0f) - (float)M_PI;
1734 }
1735
1736 float angle_wrap_deg(float angle)
1737 {
1738         return mod_inline(angle + 180.0f, 360.0f) - 180.0f;
1739 }