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