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