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