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