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