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