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