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