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