source/blender/blenlib/intern/math_rotation.c

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