Merged 40338-40364
[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
775
776 void single_axis_angle_to_mat3(float mat[3][3], const char axis, const float angle)
777 {
778         const float angle_cos= cosf(angle);
779         const float angle_sin= sinf(angle);
780
781         switch(axis) {
782         case 'X': /* rotation around X */
783                 mat[0][0] =  1.0f;
784                 mat[0][1] =  0.0f;
785                 mat[0][2] =  0.0f;
786                 mat[1][0] =  0.0f;
787                 mat[1][1] =  angle_cos;
788                 mat[1][2] =  angle_sin;
789                 mat[2][0] =  0.0f;
790                 mat[2][1] = -angle_sin;
791                 mat[2][2] =  angle_cos;
792                 break;
793         case 'Y': /* rotation around Y */
794                 mat[0][0] =  angle_cos;
795                 mat[0][1] =  0.0f;
796                 mat[0][2] = -angle_sin;
797                 mat[1][0] =  0.0f;
798                 mat[1][1] =  1.0f;
799                 mat[1][2] =  0.0f;
800                 mat[2][0] =  angle_sin;
801                 mat[2][1] =  0.0f;
802                 mat[2][2] =  angle_cos;
803                 break;
804         case 'Z': /* rotation around Z */
805                 mat[0][0] =  angle_cos;
806                 mat[0][1] =  angle_sin;
807                 mat[0][2] =  0.0f;
808                 mat[1][0] = -angle_sin;
809                 mat[1][1] =  angle_cos;
810                 mat[1][2] =  0.0f;
811                 mat[2][0] =  0.0f;
812                 mat[2][1] =  0.0f;
813                 mat[2][2] =  1.0f;
814                 break;
815         default:
816                 assert("invalid axis");
817         }
818 }
819
820 /****************************** Vector/Rotation ******************************/
821 /* TODO: the following calls should probably be depreceated sometime         */
822
823 /* axis angle to 3x3 matrix */
824 void vec_rot_to_mat3(float mat[][3], const float vec[3], const float phi)
825 {
826         /* rotation of phi radials around vec */
827         float vx, vx2, vy, vy2, vz, vz2, co, si;
828         
829         vx= vec[0];
830         vy= vec[1];
831         vz= vec[2];
832         vx2= vx*vx;
833         vy2= vy*vy;
834         vz2= vz*vz;
835         co= (float)cos(phi);
836         si= (float)sin(phi);
837         
838         mat[0][0]= vx2+co*(1.0f-vx2);
839         mat[0][1]= vx*vy*(1.0f-co)+vz*si;
840         mat[0][2]= vz*vx*(1.0f-co)-vy*si;
841         mat[1][0]= vx*vy*(1.0f-co)-vz*si;
842         mat[1][1]= vy2+co*(1.0f-vy2);
843         mat[1][2]= vy*vz*(1.0f-co)+vx*si;
844         mat[2][0]= vz*vx*(1.0f-co)+vy*si;
845         mat[2][1]= vy*vz*(1.0f-co)-vx*si;
846         mat[2][2]= vz2+co*(1.0f-vz2);
847 }
848
849 /* axis angle to 4x4 matrix */
850 void vec_rot_to_mat4(float mat[][4], const float vec[3], const float phi)
851 {
852         float tmat[3][3];
853         
854         vec_rot_to_mat3(tmat,vec, phi);
855         unit_m4(mat);
856         copy_m4_m3(mat, tmat);
857 }
858
859 /* axis angle to quaternion */
860 void vec_rot_to_quat(float *quat, const float vec[3], const float phi)
861 {
862         /* rotation of phi radials around vec */
863         float si;
864
865         quat[1]= vec[0];
866         quat[2]= vec[1];
867         quat[3]= vec[2];
868         
869         if(normalize_v3(quat+1) == 0.0f) {
870                 unit_qt(quat);
871         }
872         else {
873                 quat[0]= (float)cos((double)phi/2.0);
874                 si= (float)sin((double)phi/2.0);
875                 quat[1] *= si;
876                 quat[2] *= si;
877                 quat[3] *= si;
878         }
879 }
880
881 /******************************** XYZ Eulers *********************************/
882
883 /* XYZ order */
884 void eul_to_mat3(float mat[][3], const float eul[3])
885 {
886         double ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
887         
888         ci = cos(eul[0]); 
889         cj = cos(eul[1]); 
890         ch = cos(eul[2]);
891         si = sin(eul[0]); 
892         sj = sin(eul[1]); 
893         sh = sin(eul[2]);
894         cc = ci*ch; 
895         cs = ci*sh; 
896         sc = si*ch; 
897         ss = si*sh;
898
899         mat[0][0] = (float)(cj*ch); 
900         mat[1][0] = (float)(sj*sc-cs); 
901         mat[2][0] = (float)(sj*cc+ss);
902         mat[0][1] = (float)(cj*sh); 
903         mat[1][1] = (float)(sj*ss+cc); 
904         mat[2][1] = (float)(sj*cs-sc);
905         mat[0][2] = (float)-sj;  
906         mat[1][2] = (float)(cj*si);    
907         mat[2][2] = (float)(cj*ci);
908
909 }
910
911 /* XYZ order */
912 void eul_to_mat4(float mat[][4], const float eul[3])
913 {
914         double ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
915         
916         ci = cos(eul[0]); 
917         cj = cos(eul[1]); 
918         ch = cos(eul[2]);
919         si = sin(eul[0]); 
920         sj = sin(eul[1]); 
921         sh = sin(eul[2]);
922         cc = ci*ch; 
923         cs = ci*sh; 
924         sc = si*ch; 
925         ss = si*sh;
926
927         mat[0][0] = (float)(cj*ch); 
928         mat[1][0] = (float)(sj*sc-cs); 
929         mat[2][0] = (float)(sj*cc+ss);
930         mat[0][1] = (float)(cj*sh); 
931         mat[1][1] = (float)(sj*ss+cc); 
932         mat[2][1] = (float)(sj*cs-sc);
933         mat[0][2] = (float)-sj;  
934         mat[1][2] = (float)(cj*si);    
935         mat[2][2] = (float)(cj*ci);
936
937
938         mat[3][0]= mat[3][1]= mat[3][2]= mat[0][3]= mat[1][3]= mat[2][3]= 0.0f;
939         mat[3][3]= 1.0f;
940 }
941
942 /* returns two euler calculation methods, so we can pick the best */
943 /* XYZ order */
944 static void mat3_to_eul2(float tmat[][3], float eul1[3], float eul2[3])
945 {
946         float cy, quat[4], mat[3][3];
947         
948         mat3_to_quat(quat,tmat);
949         quat_to_mat3(mat,quat);
950         copy_m3_m3(mat, tmat);
951         normalize_m3(mat);
952         
953         cy = (float)sqrt(mat[0][0]*mat[0][0] + mat[0][1]*mat[0][1]);
954         
955         if (cy > 16.0f*FLT_EPSILON) {
956                 
957                 eul1[0] = (float)atan2(mat[1][2], mat[2][2]);
958                 eul1[1] = (float)atan2(-mat[0][2], cy);
959                 eul1[2] = (float)atan2(mat[0][1], mat[0][0]);
960                 
961                 eul2[0] = (float)atan2(-mat[1][2], -mat[2][2]);
962                 eul2[1] = (float)atan2(-mat[0][2], -cy);
963                 eul2[2] = (float)atan2(-mat[0][1], -mat[0][0]);
964                 
965         } else {
966                 eul1[0] = (float)atan2(-mat[2][1], mat[1][1]);
967                 eul1[1] = (float)atan2(-mat[0][2], cy);
968                 eul1[2] = 0.0f;
969                 
970                 copy_v3_v3(eul2, eul1);
971         }
972 }
973
974 /* XYZ order */
975 void mat3_to_eul(float *eul,float tmat[][3])
976 {
977         float eul1[3], eul2[3];
978         
979         mat3_to_eul2(tmat, eul1, eul2);
980                 
981         /* return best, which is just the one with lowest values it in */
982         if(fabs(eul1[0])+fabs(eul1[1])+fabs(eul1[2]) > fabs(eul2[0])+fabs(eul2[1])+fabs(eul2[2])) {
983                 copy_v3_v3(eul, eul2);
984         }
985         else {
986                 copy_v3_v3(eul, eul1);
987         }
988 }
989
990 /* XYZ order */
991 void mat4_to_eul(float *eul,float tmat[][4])
992 {
993         float tempMat[3][3];
994
995         copy_m3_m4(tempMat, tmat);
996         normalize_m3(tempMat);
997         mat3_to_eul(eul,tempMat);
998 }
999
1000 /* XYZ order */
1001 void quat_to_eul(float *eul, const float quat[4])
1002 {
1003         float mat[3][3];
1004
1005         quat_to_mat3(mat,quat);
1006         mat3_to_eul(eul,mat);
1007 }
1008
1009 /* XYZ order */
1010 void eul_to_quat(float *quat, const float eul[3])
1011 {
1012         float ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
1013  
1014         ti = eul[0]*0.5f; tj = eul[1]*0.5f; th = eul[2]*0.5f;
1015         ci = (float)cos(ti);  cj = (float)cos(tj);  ch = (float)cos(th);
1016         si = (float)sin(ti);  sj = (float)sin(tj);  sh = (float)sin(th);
1017         cc = ci*ch; cs = ci*sh; sc = si*ch; ss = si*sh;
1018         
1019         quat[0] = cj*cc + sj*ss;
1020         quat[1] = cj*sc - sj*cs;
1021         quat[2] = cj*ss + sj*cc;
1022         quat[3] = cj*cs - sj*sc;
1023 }
1024
1025 /* XYZ order */
1026 void rotate_eul(float *beul, const char axis, const float ang)
1027 {
1028         float eul[3], mat1[3][3], mat2[3][3], totmat[3][3];
1029
1030         assert(axis >= 'X' && axis <= 'Z');
1031
1032         eul[0]= eul[1]= eul[2]= 0.0f;
1033         if(axis=='X') eul[0]= ang;
1034         else if(axis=='Y') eul[1]= ang;
1035         else eul[2]= ang;
1036         
1037         eul_to_mat3(mat1,eul);
1038         eul_to_mat3(mat2,beul);
1039         
1040         mul_m3_m3m3(totmat, mat2, mat1);
1041         
1042         mat3_to_eul(beul,totmat);
1043         
1044 }
1045
1046 /* exported to transform.c */
1047 /* order independent! */
1048 void compatible_eul(float eul[3], const float oldrot[3])
1049 {
1050         float dx, dy, dz;
1051         
1052         /* correct differences of about 360 degrees first */
1053         dx= eul[0] - oldrot[0];
1054         dy= eul[1] - oldrot[1];
1055         dz= eul[2] - oldrot[2];
1056         
1057         while(fabs(dx) > 5.1) {
1058                 if(dx > 0.0f) eul[0] -= 2.0f*(float)M_PI; else eul[0]+= 2.0f*(float)M_PI;
1059                 dx= eul[0] - oldrot[0];
1060         }
1061         while(fabs(dy) > 5.1) {
1062                 if(dy > 0.0f) eul[1] -= 2.0f*(float)M_PI; else eul[1]+= 2.0f*(float)M_PI;
1063                 dy= eul[1] - oldrot[1];
1064         }
1065         while(fabs(dz) > 5.1) {
1066                 if(dz > 0.0f) eul[2] -= 2.0f*(float)M_PI; else eul[2]+= 2.0f*(float)M_PI;
1067                 dz= eul[2] - oldrot[2];
1068         }
1069         
1070         /* is 1 of the axis rotations larger than 180 degrees and the other small? NO ELSE IF!! */      
1071         if(fabs(dx) > 3.2 && fabs(dy)<1.6 && fabs(dz)<1.6) {
1072                 if(dx > 0.0f) eul[0] -= 2.0f*(float)M_PI; else eul[0]+= 2.0f*(float)M_PI;
1073         }
1074         if(fabs(dy) > 3.2 && fabs(dz)<1.6 && fabs(dx)<1.6) {
1075                 if(dy > 0.0f) eul[1] -= 2.0f*(float)M_PI; else eul[1]+= 2.0f*(float)M_PI;
1076         }
1077         if(fabs(dz) > 3.2 && fabs(dx)<1.6 && fabs(dy)<1.6) {
1078                 if(dz > 0.0f) eul[2] -= 2.0f*(float)M_PI; else eul[2]+= 2.0f*(float)M_PI;
1079         }
1080         
1081         /* the method below was there from ancient days... but why! probably because the code sucks :)
1082                 */
1083 #if 0   
1084         /* calc again */
1085         dx= eul[0] - oldrot[0];
1086         dy= eul[1] - oldrot[1];
1087         dz= eul[2] - oldrot[2];
1088         
1089         /* special case, tested for x-z  */
1090         
1091         if((fabs(dx) > 3.1 && fabs(dz) > 1.5) || (fabs(dx) > 1.5 && fabs(dz) > 3.1)) {
1092                 if(dx > 0.0) eul[0] -= M_PI; else eul[0]+= M_PI;
1093                 if(eul[1] > 0.0) eul[1]= M_PI - eul[1]; else eul[1]= -M_PI - eul[1];
1094                 if(dz > 0.0) eul[2] -= M_PI; else eul[2]+= M_PI;
1095                 
1096         }
1097         else if((fabs(dx) > 3.1 && fabs(dy) > 1.5) || (fabs(dx) > 1.5 && fabs(dy) > 3.1)) {
1098                 if(dx > 0.0) eul[0] -= M_PI; else eul[0]+= M_PI;
1099                 if(dy > 0.0) eul[1] -= M_PI; else eul[1]+= M_PI;
1100                 if(eul[2] > 0.0) eul[2]= M_PI - eul[2]; else eul[2]= -M_PI - eul[2];
1101         }
1102         else if((fabs(dy) > 3.1 && fabs(dz) > 1.5) || (fabs(dy) > 1.5 && fabs(dz) > 3.1)) {
1103                 if(eul[0] > 0.0) eul[0]= M_PI - eul[0]; else eul[0]= -M_PI - eul[0];
1104                 if(dy > 0.0) eul[1] -= M_PI; else eul[1]+= M_PI;
1105                 if(dz > 0.0) eul[2] -= M_PI; else eul[2]+= M_PI;
1106         }
1107 #endif  
1108 }
1109
1110 /* uses 2 methods to retrieve eulers, and picks the closest */
1111 /* XYZ order */
1112 void mat3_to_compatible_eul(float eul[3], const float oldrot[3], float mat[][3])
1113 {
1114         float eul1[3], eul2[3];
1115         float d1, d2;
1116         
1117         mat3_to_eul2(mat, eul1, eul2);
1118         
1119         compatible_eul(eul1, oldrot);
1120         compatible_eul(eul2, oldrot);
1121         
1122         d1= (float)fabs(eul1[0]-oldrot[0]) + (float)fabs(eul1[1]-oldrot[1]) + (float)fabs(eul1[2]-oldrot[2]);
1123         d2= (float)fabs(eul2[0]-oldrot[0]) + (float)fabs(eul2[1]-oldrot[1]) + (float)fabs(eul2[2]-oldrot[2]);
1124         
1125         /* return best, which is just the one with lowest difference */
1126         if(d1 > d2) {
1127                 copy_v3_v3(eul, eul2);
1128         }
1129         else {
1130                 copy_v3_v3(eul, eul1);
1131         }
1132         
1133 }
1134
1135 /************************** Arbitrary Order Eulers ***************************/
1136
1137 /* Euler Rotation Order Code:
1138  * was adapted from  
1139                   ANSI C code from the article
1140                 "Euler Angle Conversion"
1141                 by Ken Shoemake, shoemake@graphics.cis.upenn.edu
1142                 in "Graphics Gems IV", Academic Press, 1994
1143  * for use in Blender
1144  */
1145
1146 /* Type for rotation order info - see wiki for derivation details */
1147 typedef struct RotOrderInfo {
1148         short axis[3];
1149         short parity;   /* parity of axis permutation (even=0, odd=1) - 'n' in original code */
1150 } RotOrderInfo;
1151
1152 /* Array of info for Rotation Order calculations 
1153  * WARNING: must be kept in same order as eEulerRotationOrders
1154  */
1155 static RotOrderInfo rotOrders[]= {
1156         /* i, j, k, n */
1157         {{0, 1, 2}, 0}, // XYZ
1158         {{0, 2, 1}, 1}, // XZY
1159         {{1, 0, 2}, 1}, // YXZ
1160         {{1, 2, 0}, 0}, // YZX
1161         {{2, 0, 1}, 0}, // ZXY
1162         {{2, 1, 0}, 1}  // ZYX
1163 };
1164
1165 /* Get relevant pointer to rotation order set from the array 
1166  * NOTE: since we start at 1 for the values, but arrays index from 0, 
1167  *               there is -1 factor involved in this process...
1168  */
1169 #define GET_ROTATIONORDER_INFO(order) (assert(order>=0 && order<=6), (order < 1) ? &rotOrders[0] : &rotOrders[(order)-1])
1170
1171 /* Construct quaternion from Euler angles (in radians). */
1172 void eulO_to_quat(float q[4], const float e[3], const short order)
1173 {
1174         RotOrderInfo *R= GET_ROTATIONORDER_INFO(order); 
1175         short i=R->axis[0],  j=R->axis[1],      k=R->axis[2];
1176         double ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
1177         double a[3];
1178         
1179         ti = e[i] * 0.5f;
1180         tj = e[j] * (R->parity ? -0.5f : 0.5f);
1181         th = e[k] * 0.5f;
1182
1183         ci = cos(ti);  cj = cos(tj);  ch = cos(th);
1184         si = sin(ti);  sj = sin(tj);  sh = sin(th);
1185         
1186         cc = ci*ch; cs = ci*sh; 
1187         sc = si*ch; ss = si*sh;
1188         
1189         a[i] = cj*sc - sj*cs;
1190         a[j] = cj*ss + sj*cc;
1191         a[k] = cj*cs - sj*sc;
1192         
1193         q[0] = cj*cc + sj*ss;
1194         q[1] = a[0];
1195         q[2] = a[1];
1196         q[3] = a[2];
1197         
1198         if (R->parity) q[j+1] = -q[j+1];
1199 }
1200
1201 /* Convert quaternion to Euler angles (in radians). */
1202 void quat_to_eulO(float e[3], short const order, const float q[4])
1203 {
1204         float M[3][3];
1205         
1206         quat_to_mat3(M,q);
1207         mat3_to_eulO(e, order,M);
1208 }
1209
1210 /* Construct 3x3 matrix from Euler angles (in radians). */
1211 void eulO_to_mat3(float M[3][3], const float e[3], const short order)
1212 {
1213         RotOrderInfo *R= GET_ROTATIONORDER_INFO(order); 
1214         short i=R->axis[0],  j=R->axis[1],      k=R->axis[2];
1215         double ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
1216         
1217         if (R->parity) {
1218                 ti = -e[i];       tj = -e[j];   th = -e[k];
1219         }
1220         else {
1221                 ti = e[i];        tj = e[j];    th = e[k];
1222         }
1223         
1224         ci = cos(ti); cj = cos(tj); ch = cos(th);
1225         si = sin(ti); sj = sin(tj); sh = sin(th);
1226         
1227         cc = ci*ch; cs = ci*sh; 
1228         sc = si*ch; ss = si*sh;
1229         
1230         M[i][i] = cj*ch; M[j][i] = sj*sc-cs; M[k][i] = sj*cc+ss;
1231         M[i][j] = cj*sh; M[j][j] = sj*ss+cc; M[k][j] = sj*cs-sc;
1232         M[i][k] = -sj;   M[j][k] = cj*si;        M[k][k] = cj*ci;
1233 }
1234
1235 /* returns two euler calculation methods, so we can pick the best */
1236 static void mat3_to_eulo2(float M[3][3], float *e1, float *e2, short order)
1237 {
1238         RotOrderInfo *R= GET_ROTATIONORDER_INFO(order); 
1239         short i=R->axis[0],  j=R->axis[1],      k=R->axis[2];
1240         float m[3][3];
1241         double cy;
1242         
1243         /* process the matrix first */
1244         copy_m3_m3(m, M);
1245         normalize_m3(m);
1246         
1247         cy= sqrt(m[i][i]*m[i][i] + m[i][j]*m[i][j]);
1248         
1249         if (cy > 16.0*(double)FLT_EPSILON) {
1250                 e1[i] = atan2(m[j][k], m[k][k]);
1251                 e1[j] = atan2(-m[i][k], cy);
1252                 e1[k] = atan2(m[i][j], m[i][i]);
1253                 
1254                 e2[i] = atan2(-m[j][k], -m[k][k]);
1255                 e2[j] = atan2(-m[i][k], -cy);
1256                 e2[k] = atan2(-m[i][j], -m[i][i]);
1257         } 
1258         else {
1259                 e1[i] = atan2(-m[k][j], m[j][j]);
1260                 e1[j] = atan2(-m[i][k], cy);
1261                 e1[k] = 0;
1262                 
1263                 copy_v3_v3(e2, e1);
1264         }
1265         
1266         if (R->parity) {
1267                 e1[0] = -e1[0]; 
1268                 e1[1] = -e1[1]; 
1269                 e1[2] = -e1[2];
1270                 
1271                 e2[0] = -e2[0]; 
1272                 e2[1] = -e2[1]; 
1273                 e2[2] = -e2[2];
1274         }
1275 }
1276
1277 /* Construct 4x4 matrix from Euler angles (in radians). */
1278 void eulO_to_mat4(float M[4][4], const float e[3], const short order)
1279 {
1280         float m[3][3];
1281         
1282         /* for now, we'll just do this the slow way (i.e. copying matrices) */
1283         normalize_m3(m);
1284         eulO_to_mat3(m,e, order);
1285         copy_m4_m3(M, m);
1286 }
1287
1288
1289 /* Convert 3x3 matrix to Euler angles (in radians). */
1290 void mat3_to_eulO(float eul[3], const short order,float M[3][3])
1291 {
1292         float eul1[3], eul2[3];
1293         
1294         mat3_to_eulo2(M, eul1, eul2, order);
1295                 
1296         /* return best, which is just the one with lowest values it in */
1297         if(fabs(eul1[0])+fabs(eul1[1])+fabs(eul1[2]) > fabs(eul2[0])+fabs(eul2[1])+fabs(eul2[2])) {
1298                 copy_v3_v3(eul, eul2);
1299         }
1300         else {
1301                 copy_v3_v3(eul, eul1);
1302         }
1303 }
1304
1305 /* Convert 4x4 matrix to Euler angles (in radians). */
1306 void mat4_to_eulO(float e[3], const short order,float M[4][4])
1307 {
1308         float m[3][3];
1309         
1310         /* for now, we'll just do this the slow way (i.e. copying matrices) */
1311         copy_m3_m4(m, M);
1312         normalize_m3(m);
1313         mat3_to_eulO(e, order,m);
1314 }
1315
1316 /* uses 2 methods to retrieve eulers, and picks the closest */
1317 void mat3_to_compatible_eulO(float eul[3], float oldrot[3], short order,float mat[3][3])
1318 {
1319         float eul1[3], eul2[3];
1320         float d1, d2;
1321         
1322         mat3_to_eulo2(mat, eul1, eul2, order);
1323         
1324         compatible_eul(eul1, oldrot);
1325         compatible_eul(eul2, oldrot);
1326         
1327         d1= (float)fabs(eul1[0]-oldrot[0]) + (float)fabs(eul1[1]-oldrot[1]) + (float)fabs(eul1[2]-oldrot[2]);
1328         d2= (float)fabs(eul2[0]-oldrot[0]) + (float)fabs(eul2[1]-oldrot[1]) + (float)fabs(eul2[2]-oldrot[2]);
1329         
1330         /* return best, which is just the one with lowest difference */
1331         if (d1 > d2)
1332                 copy_v3_v3(eul, eul2);
1333         else
1334                 copy_v3_v3(eul, eul1);
1335 }
1336
1337 void mat4_to_compatible_eulO(float eul[3], float oldrot[3], short order,float M[4][4])
1338 {
1339         float m[3][3];
1340         
1341         /* for now, we'll just do this the slow way (i.e. copying matrices) */
1342         copy_m3_m4(m, M);
1343         normalize_m3(m);
1344         mat3_to_compatible_eulO(eul, oldrot, order, m);
1345 }
1346 /* rotate the given euler by the given angle on the specified axis */
1347 // NOTE: is this safe to do with different axis orders?
1348 void rotate_eulO(float beul[3], short order, char axis, float ang)
1349 {
1350         float eul[3], mat1[3][3], mat2[3][3], totmat[3][3];
1351
1352         assert(axis >= 'X' && axis <= 'Z');
1353
1354         eul[0]= eul[1]= eul[2]= 0.0f;
1355         if (axis=='X') 
1356                 eul[0]= ang;
1357         else if (axis=='Y')
1358                 eul[1]= ang;
1359         else 
1360                 eul[2]= ang;
1361         
1362         eulO_to_mat3(mat1,eul, order);
1363         eulO_to_mat3(mat2,beul, order);
1364         
1365         mul_m3_m3m3(totmat, mat2, mat1);
1366         
1367         mat3_to_eulO(beul, order,totmat);
1368 }
1369
1370 /* the matrix is written to as 3 axis vectors */
1371 void eulO_to_gimbal_axis(float gmat[][3], const float eul[3], const short order)
1372 {
1373         RotOrderInfo *R= GET_ROTATIONORDER_INFO(order);
1374
1375         float mat[3][3];
1376         float teul[3];
1377
1378         /* first axis is local */
1379         eulO_to_mat3(mat,eul, order);
1380         copy_v3_v3(gmat[R->axis[0]], mat[R->axis[0]]);
1381         
1382         /* second axis is local minus first rotation */
1383         copy_v3_v3(teul, eul);
1384         teul[R->axis[0]] = 0;
1385         eulO_to_mat3(mat,teul, order);
1386         copy_v3_v3(gmat[R->axis[1]], mat[R->axis[1]]);
1387         
1388         
1389         /* Last axis is global */
1390         gmat[R->axis[2]][0] = 0;
1391         gmat[R->axis[2]][1] = 0;
1392         gmat[R->axis[2]][2] = 0;
1393         gmat[R->axis[2]][R->axis[2]] = 1;
1394 }
1395
1396 /******************************* Dual Quaternions ****************************/
1397
1398 /*
1399    Conversion routines between (regular quaternion, translation) and
1400    dual quaternion.
1401
1402    Version 1.0.0, February 7th, 2007
1403
1404    Copyright (C) 2006-2007 University of Dublin, Trinity College, All Rights 
1405    Reserved
1406
1407    This software is provided 'as-is', without any express or implied
1408    warranty.  In no event will the author(s) be held liable for any damages
1409    arising from the use of this software.
1410
1411    Permission is granted to anyone to use this software for any purpose,
1412    including commercial applications, and to alter it and redistribute it
1413    freely, subject to the following restrictions:
1414
1415    1. The origin of this software must not be misrepresented; you must not
1416           claim that you wrote the original software. If you use this software
1417           in a product, an acknowledgment in the product documentation would be
1418           appreciated but is not required.
1419    2. Altered source versions must be plainly marked as such, and must not be
1420           misrepresented as being the original software.
1421    3. This notice may not be removed or altered from any source distribution.
1422
1423    Author: Ladislav Kavan, kavanl@cs.tcd.ie
1424
1425    Changes for Blender:
1426    - renaming, style changes and optimizations
1427    - added support for scaling
1428 */
1429
1430 void mat4_to_dquat(DualQuat *dq,float basemat[][4], float mat[][4])
1431 {
1432         float *t, *q, dscale[3], scale[3], basequat[4];
1433         float baseRS[4][4], baseinv[4][4], baseR[4][4], baseRinv[4][4];
1434         float R[4][4], S[4][4];
1435
1436         /* split scaling and rotation, there is probably a faster way to do
1437            this, it's done like this now to correctly get negative scaling */
1438         mul_m4_m4m4(baseRS, basemat, mat);
1439         mat4_to_size(scale,baseRS);
1440
1441         copy_v3_v3(dscale, scale);
1442         dscale[0] -= 1.0f; dscale[1] -= 1.0f; dscale[2] -= 1.0f;
1443
1444         if((determinant_m4(mat) < 0.0f) || len_v3(dscale) > 1e-4f) {
1445                 /* extract R and S  */
1446                 float tmp[4][4];
1447
1448                  /* extra orthogonalize, to avoid flipping with stretched bones */
1449                 copy_m4_m4(tmp, baseRS);
1450                 orthogonalize_m4(tmp, 1);
1451                 mat4_to_quat(basequat, tmp);
1452
1453                 quat_to_mat4(baseR, basequat);
1454                 copy_v3_v3(baseR[3], baseRS[3]);
1455
1456                 invert_m4_m4(baseinv, basemat);
1457                 mul_m4_m4m4(R, baseinv, baseR);
1458
1459                 invert_m4_m4(baseRinv, baseR);
1460                 mul_m4_m4m4(S, baseRS, baseRinv);
1461
1462                 /* set scaling part */
1463                 mul_serie_m4(dq->scale, basemat, S, baseinv, NULL, NULL, NULL, NULL, NULL);
1464                 dq->scale_weight= 1.0f;
1465         }
1466         else {
1467                 /* matrix does not contain scaling */
1468                 copy_m4_m4(R, mat);
1469                 dq->scale_weight= 0.0f;
1470         }
1471
1472         /* non-dual part */
1473         mat4_to_quat(dq->quat,R);
1474
1475         /* dual part */
1476         t= R[3];
1477         q= dq->quat;
1478         dq->trans[0]= -0.5f*(t[0]*q[1] + t[1]*q[2] + t[2]*q[3]);
1479         dq->trans[1]=  0.5f*(t[0]*q[0] + t[1]*q[3] - t[2]*q[2]);
1480         dq->trans[2]=  0.5f*(-t[0]*q[3] + t[1]*q[0] + t[2]*q[1]);
1481         dq->trans[3]=  0.5f*(t[0]*q[2] - t[1]*q[1] + t[2]*q[0]);
1482 }
1483
1484 void dquat_to_mat4(float mat[][4], DualQuat *dq)
1485 {
1486         float len, *t, q0[4];
1487         
1488         /* regular quaternion */
1489         copy_qt_qt(q0, dq->quat);
1490
1491         /* normalize */
1492         len= (float)sqrt(dot_qtqt(q0, q0)); 
1493         if(len != 0.0f)
1494                 mul_qt_fl(q0, 1.0f/len);
1495         
1496         /* rotation */
1497         quat_to_mat4(mat,q0);
1498
1499         /* translation */
1500         t= dq->trans;
1501         mat[3][0]= 2.0f*(-t[0]*q0[1] + t[1]*q0[0] - t[2]*q0[3] + t[3]*q0[2]);
1502         mat[3][1]= 2.0f*(-t[0]*q0[2] + t[1]*q0[3] + t[2]*q0[0] - t[3]*q0[1]);
1503         mat[3][2]= 2.0f*(-t[0]*q0[3] - t[1]*q0[2] + t[2]*q0[1] + t[3]*q0[0]);
1504
1505         /* note: this does not handle scaling */
1506 }       
1507
1508 void add_weighted_dq_dq(DualQuat *dqsum, DualQuat *dq, float weight)
1509 {
1510         int flipped= 0;
1511
1512         /* make sure we interpolate quats in the right direction */
1513         if (dot_qtqt(dq->quat, dqsum->quat) < 0) {
1514                 flipped= 1;
1515                 weight= -weight;
1516         }
1517
1518         /* interpolate rotation and translation */
1519         dqsum->quat[0] += weight*dq->quat[0];
1520         dqsum->quat[1] += weight*dq->quat[1];
1521         dqsum->quat[2] += weight*dq->quat[2];
1522         dqsum->quat[3] += weight*dq->quat[3];
1523
1524         dqsum->trans[0] += weight*dq->trans[0];
1525         dqsum->trans[1] += weight*dq->trans[1];
1526         dqsum->trans[2] += weight*dq->trans[2];
1527         dqsum->trans[3] += weight*dq->trans[3];
1528
1529         /* interpolate scale - but only if needed */
1530         if (dq->scale_weight) {
1531                 float wmat[4][4];
1532                 
1533                 if(flipped)     /* we don't want negative weights for scaling */
1534                         weight= -weight;
1535                 
1536                 copy_m4_m4(wmat, dq->scale);
1537                 mul_m4_fl(wmat, weight);
1538                 add_m4_m4m4(dqsum->scale, dqsum->scale, wmat);
1539                 dqsum->scale_weight += weight;
1540         }
1541 }
1542
1543 void normalize_dq(DualQuat *dq, float totweight)
1544 {
1545         float scale= 1.0f/totweight;
1546
1547         mul_qt_fl(dq->quat, scale);
1548         mul_qt_fl(dq->trans, scale);
1549         
1550         if(dq->scale_weight) {
1551                 float addweight= totweight - dq->scale_weight;
1552                 
1553                 if(addweight) {
1554                         dq->scale[0][0] += addweight;
1555                         dq->scale[1][1] += addweight;
1556                         dq->scale[2][2] += addweight;
1557                         dq->scale[3][3] += addweight;
1558                 }
1559
1560                 mul_m4_fl(dq->scale, scale);
1561                 dq->scale_weight= 1.0f;
1562         }
1563 }
1564
1565 void mul_v3m3_dq(float *co, float mat[][3],DualQuat *dq)
1566 {       
1567         float M[3][3], t[3], scalemat[3][3], len2;
1568         float w= dq->quat[0], x= dq->quat[1], y= dq->quat[2], z= dq->quat[3];
1569         float t0= dq->trans[0], t1= dq->trans[1], t2= dq->trans[2], t3= dq->trans[3];
1570         
1571         /* rotation matrix */
1572         M[0][0]= w*w + x*x - y*y - z*z;
1573         M[1][0]= 2*(x*y - w*z);
1574         M[2][0]= 2*(x*z + w*y);
1575
1576         M[0][1]= 2*(x*y + w*z);
1577         M[1][1]= w*w + y*y - x*x - z*z;
1578         M[2][1]= 2*(y*z - w*x); 
1579
1580         M[0][2]= 2*(x*z - w*y);
1581         M[1][2]= 2*(y*z + w*x);
1582         M[2][2]= w*w + z*z - x*x - y*y;
1583         
1584         len2= dot_qtqt(dq->quat, dq->quat);
1585         if(len2 > 0.0f)
1586                 len2= 1.0f/len2;
1587         
1588         /* translation */
1589         t[0]= 2*(-t0*x + w*t1 - t2*z + y*t3);
1590         t[1]= 2*(-t0*y + t1*z - x*t3 + w*t2);
1591         t[2]= 2*(-t0*z + x*t2 + w*t3 - t1*y);
1592
1593         /* apply scaling */
1594         if(dq->scale_weight)
1595                 mul_m4_v3(dq->scale, co);
1596         
1597         /* apply rotation and translation */
1598         mul_m3_v3(M, co);
1599         co[0]= (co[0] + t[0])*len2;
1600         co[1]= (co[1] + t[1])*len2;
1601         co[2]= (co[2] + t[2])*len2;
1602
1603         /* compute crazyspace correction mat */
1604         if(mat) {
1605                 if(dq->scale_weight) {
1606                         copy_m3_m4(scalemat, dq->scale);
1607                         mul_m3_m3m3(mat, M, scalemat);
1608                 }
1609                 else
1610                         copy_m3_m3(mat, M);
1611                 mul_m3_fl(mat, len2);
1612         }
1613 }
1614
1615 void copy_dq_dq(DualQuat *dq1, DualQuat *dq2)
1616 {
1617         memcpy(dq1, dq2, sizeof(DualQuat));
1618 }
1619
1620 /* axis matches eTrackToAxis_Modes */
1621 void quat_apply_track(float quat[4], short axis, short upflag)
1622 {       
1623         /* rotations are hard coded to match vec_to_quat */
1624         const float quat_track[][4]= {{0.70710676908493, 0.0, -0.70710676908493, 0.0},  /* pos-y90 */ 
1625                                       {0.5, 0.5, 0.5, 0.5},  /* Quaternion((1,0,0), radians(90)) * Quaternion((0,1,0), radians(90)) */ 
1626                                       {0.70710676908493, 0.0, 0.0, 0.70710676908493},  /* pos-z90 */ 
1627                                       {0.70710676908493, 0.0, 0.70710676908493, 0.0}, /* neg-y90 */ 
1628                                       {0.5, -0.5, -0.5, 0.5}, /* Quaternion((1,0,0), radians(-90)) * Quaternion((0,1,0), radians(-90)) */ 
1629                                       {-3.0908619663705394e-08, 0.70710676908493, 0.70710676908493, 3.0908619663705394e-08}}; /* no rotation */
1630
1631         assert(axis >= 0 && axis <= 5);
1632         assert(upflag >= 0 && upflag <= 2);
1633         
1634         mul_qt_qtqt(quat, quat, quat_track[axis]);
1635
1636         if(axis>2)
1637                 axis= axis-3;
1638
1639         /* there are 2 possible up-axis for each axis used, the 'quat_track' applies so the first
1640          * up axis is used X->Y, Y->X, Z->X, if this first up axis isn used then rotate 90d
1641          * the strange bit shift below just find the low axis {X:Y, Y:X, Z:X} */
1642         if(upflag != (2-axis)>>1) {
1643                 float q[4]= {0.70710676908493, 0.0, 0.0, 0.0}; /* assign 90d rotation axis */
1644                 q[axis+1] = ((axis==1)) ? 0.70710676908493 : -0.70710676908493; /* flip non Y axis */
1645                 mul_qt_qtqt(quat, quat, q);
1646         }
1647 }
1648
1649
1650 void vec_apply_track(float vec[3], short axis)
1651 {
1652         float tvec[3];
1653
1654         assert(axis >= 0 && axis <= 5);
1655         
1656         copy_v3_v3(tvec, vec);
1657
1658         switch(axis) {
1659         case 0: /* pos-x */
1660                 /* vec[0]=  0.0; */
1661                 vec[1]=  tvec[2];
1662                 vec[2]=  -tvec[1];
1663                 break;
1664         case 1: /* pos-y */
1665                 /* vec[0]= tvec[0]; */
1666                 /* vec[1]=  0.0; */
1667                 /* vec[2]= tvec[2]; */ 
1668                 break;
1669         case 2: /* pos-z */
1670                 /* vec[0]= tvec[0]; */
1671                 /* vec[1]= tvec[1]; */
1672                 // vec[2]=  0.0; */
1673                 break;
1674         case 3: /* neg-x */
1675                 /* vec[0]=  0.0; */
1676                 vec[1]=  tvec[2];
1677                 vec[2]= -tvec[1];
1678                 break;
1679         case 4: /* neg-y */
1680                 vec[0]= -tvec[2];
1681                 /* vec[1]=  0.0; */
1682                 vec[2]= tvec[0];
1683                 break;
1684         case 5: /* neg-z */
1685                 vec[0]= -tvec[0];
1686                 vec[1]= -tvec[1];
1687                 /* vec[2]=  0.0; */
1688                 break;
1689         }
1690 }
1691
1692 /* lens/angle conversion (radians) */
1693 float lens_to_angle(float lens)
1694 {
1695         return 2.0f * atanf(16.0f/lens);
1696 }
1697
1698 float angle_to_lens(float angle)
1699 {
1700         return 16.0f / tanf(angle * 0.5f);
1701 }
1702
1703 /* 'mod_inline(-3,4)= 1', 'fmod(-3,4)= -3' */
1704 static float mod_inline(float a, float b)
1705 {
1706         return a - (b * floorf(a / b));
1707 }
1708
1709 float angle_wrap_rad(float angle)
1710 {
1711         return mod_inline(angle + (float)M_PI, (float)M_PI*2.0f) - (float)M_PI;
1712 }
1713
1714 float angle_wrap_deg(float angle)
1715 {
1716         return mod_inline(angle + 180.0f, 360.0f) - 180.0f;
1717 }