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