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