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