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