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