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