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