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