source/blender/blenlib/intern/math_rotation.c

   1 /*
   2  * ***** BEGIN GPL LICENSE BLOCK *****
   3  *
   4  * This program is free software; you can redistribute it and/or
   5  * modify it under the terms of the GNU General Public License
   6  * as published by the Free Software Foundation; either version 2
   7  * of the License, or (at your option) any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with this program; if not, write to the Free Software Foundation,
  16  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
  17  *
  18  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
  19  * All rights reserved.
  20
  21  * The Original Code is: some of this file.
  22  *
  23  * ***** END GPL LICENSE BLOCK *****
  24  * */
  25
  26 /** \file blender/blenlib/intern/math_rotation.c
  27  *  \ingroup bli
  28  */
  29
  30 #include <assert.h>
  31 #include "BLI_math.h"
  32
  33 #include "BLI_strict_flags.h"
  34
  35 /******************************** Quaternions ********************************/
  36
  37 /* used to test is a quat is not normalized (only used for debug prints) */
  38 #ifdef DEBUG
  39 #  define QUAT_EPSILON 0.0001
  40 #endif
  41
  42 /* convenience, avoids setting Y axis everywhere */
  43 void unit_axis_angle(float axis[3], float *angle)
  44 {
  45         axis[0] = 0.0f;
  46         axis[1] = 1.0f;
  47         axis[2] = 0.0f;
  48         *angle = 0.0f;
  49 }
  50
  51 void unit_qt(float q[4])
  52 {
  53         q[0] = 1.0f;
  54         q[1] = q[2] = q[3] = 0.0f;
  55 }
  56
  57 void copy_qt_qt(float q1[4], const float q2[4])
  58 {
  59         q1[0] = q2[0];
  60         q1[1] = q2[1];
  61         q1[2] = q2[2];
  62         q1[3] = q2[3];
  63 }
  64
  65 bool is_zero_qt(const float q[4])
  66 {
  67         return (q[0] == 0 && q[1] == 0 && q[2] == 0 && q[3] == 0);
  68 }
  69
  70 void mul_qt_qtqt(float q[4], const float q1[4], const float q2[4])
  71 {
  72         float t0, t1, t2;
  73
  74         t0 = q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3];
  75         t1 = q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2];
  76         t2 = q1[0] * q2[2] + q1[2] * q2[0] + q1[3] * q2[1] - q1[1] * q2[3];
  77         q[3] = q1[0] * q2[3] + q1[3] * q2[0] + q1[1] * q2[2] - q1[2] * q2[1];
  78         q[0] = t0;
  79         q[1] = t1;
  80         q[2] = t2;
  81 }
  82
  83 /**
  84  * \note:
  85  * Assumes a unit quaternion?
  86  *
  87  * \note: multiplying by 3x3 matrix is ~25% faster.
  88  *
  89  * in fact not, but you may want to use a unit quat, read on...
  90  *
  91  * Shortcut for 'q v q*' when \a v is actually a quaternion.
  92  * This removes the need for converting a vector to a quaternion,
  93  * calculating q's conjugate and converting back to a vector.
  94  * It also happens to be faster (17+,24* vs * 24+,32*).
  95  * If \a q is not a unit quaternion, then \a v will be both rotated by
  96  * the same amount as if q was a unit quaternion, and scaled by the square of
  97  * the length of q.
  98  *
  99  * For people used to python mathutils, its like:
 100  * def mul_qt_v3(q, v): (q * Quaternion((0.0, v[0], v[1], v[2])) * q.conjugated())[1:]
 101  */
 102 void mul_qt_v3(const float q[4], float v[3])
 103 {
 104         float t0, t1, t2;
 105
 106         t0 = -q[1] * v[0] - q[2] * v[1] - q[3] * v[2];
 107         t1 = q[0] * v[0] + q[2] * v[2] - q[3] * v[1];
 108         t2 = q[0] * v[1] + q[3] * v[0] - q[1] * v[2];
 109         v[2] = q[0] * v[2] + q[1] * v[1] - q[2] * v[0];
 110         v[0] = t1;
 111         v[1] = t2;
 112
 113         t1 = t0 * -q[1] + v[0] * q[0] - v[1] * q[3] + v[2] * q[2];
 114         t2 = t0 * -q[2] + v[1] * q[0] - v[2] * q[1] + v[0] * q[3];
 115         v[2] = t0 * -q[3] + v[2] * q[0] - v[0] * q[2] + v[1] * q[1];
 116         v[0] = t1;
 117         v[1] = t2;
 118 }
 119
 120 void conjugate_qt_qt(float q1[4], const float q2[4])
 121 {
 122         q1[0] =  q2[0];
 123         q1[1] = -q2[1];
 124         q1[2] = -q2[2];
 125         q1[3] = -q2[3];
 126 }
 127
 128 void conjugate_qt(float q[4])
 129 {
 130         q[1] = -q[1];
 131         q[2] = -q[2];
 132         q[3] = -q[3];
 133 }
 134
 135 float dot_qtqt(const float q1[4], const float q2[4])
 136 {
 137         return q1[0] * q2[0] + q1[1] * q2[1] + q1[2] * q2[2] + q1[3] * q2[3];
 138 }
 139
 140 void invert_qt(float q[4])
 141 {
 142         float f = dot_qtqt(q, q);
 143
 144         if (f == 0.0f)
 145                 return;
 146
 147         conjugate_qt(q);
 148         mul_qt_fl(q, 1.0f / f);
 149 }
 150
 151 void invert_qt_qt(float q1[4], const float q2[4])
 152 {
 153         copy_qt_qt(q1, q2);
 154         invert_qt(q1);
 155 }
 156
 157 /* simple mult */
 158 void mul_qt_fl(float q[4], const float f)
 159 {
 160         q[0] *= f;
 161         q[1] *= f;
 162         q[2] *= f;
 163         q[3] *= f;
 164 }
 165
 166 void sub_qt_qtqt(float q[4], const float q1[4], const float q2[4])
 167 {
 168         float nq2[4];
 169
 170         nq2[0] = -q2[0];
 171         nq2[1] = q2[1];
 172         nq2[2] = q2[2];
 173         nq2[3] = q2[3];
 174
 175         mul_qt_qtqt(q, q1, nq2);
 176 }
 177
 178 /* angular mult factor */
 179 void mul_fac_qt_fl(float q[4], const float fac)
 180 {
 181         const float angle = fac * saacos(q[0]); /* quat[0] = cos(0.5 * angle), but now the 0.5 and 2.0 rule out */
 182         const float co = cosf(angle);
 183         const float si = sinf(angle);
 184         q[0] = co;
 185         normalize_v3(q + 1);
 186         mul_v3_fl(q + 1, si);
 187 }
 188
 189 /* skip error check, currently only needed by mat3_to_quat_is_ok */
 190 static void quat_to_mat3_no_error(float m[3][3], const float q[4])
 191 {
 192         double q0, q1, q2, q3, qda, qdb, qdc, qaa, qab, qac, qbb, qbc, qcc;
 193
 194         q0 = M_SQRT2 * (double)q[0];
 195         q1 = M_SQRT2 * (double)q[1];
 196         q2 = M_SQRT2 * (double)q[2];
 197         q3 = M_SQRT2 * (double)q[3];
 198
 199         qda = q0 * q1;
 200         qdb = q0 * q2;
 201         qdc = q0 * q3;
 202         qaa = q1 * q1;
 203         qab = q1 * q2;
 204         qac = q1 * q3;
 205         qbb = q2 * q2;
 206         qbc = q2 * q3;
 207         qcc = q3 * q3;
 208
 209         m[0][0] = (float)(1.0 - qbb - qcc);
 210         m[0][1] = (float)(qdc + qab);
 211         m[0][2] = (float)(-qdb + qac);
 212
 213         m[1][0] = (float)(-qdc + qab);
 214         m[1][1] = (float)(1.0 - qaa - qcc);
 215         m[1][2] = (float)(qda + qbc);
 216
 217         m[2][0] = (float)(qdb + qac);
 218         m[2][1] = (float)(-qda + qbc);
 219         m[2][2] = (float)(1.0 - qaa - qbb);
 220 }
 221
 222 void quat_to_mat3(float m[3][3], const float q[4])
 223 {
 224 #ifdef DEBUG
 225         float f;
 226         if (!((f = dot_qtqt(q, q)) == 0.0f || (fabsf(f - 1.0f) < (float)QUAT_EPSILON))) {
 227                 fprintf(stderr, "Warning! quat_to_mat3() called with non-normalized: size %.8f *** report a bug ***\n", f);
 228         }
 229 #endif
 230
 231         quat_to_mat3_no_error(m, q);
 232 }
 233
 234 void quat_to_mat4(float m[4][4], const float q[4])
 235 {
 236         double q0, q1, q2, q3, qda, qdb, qdc, qaa, qab, qac, qbb, qbc, qcc;
 237
 238 #ifdef DEBUG
 239         if (!((q0 = dot_qtqt(q, q)) == 0.0 || (fabs(q0 - 1.0) < QUAT_EPSILON))) {
 240                 fprintf(stderr, "Warning! quat_to_mat4() called with non-normalized: size %.8f *** report a bug ***\n", (float)q0);
 241         }
 242 #endif
 243
 244         q0 = M_SQRT2 * (double)q[0];
 245         q1 = M_SQRT2 * (double)q[1];
 246         q2 = M_SQRT2 * (double)q[2];
 247         q3 = M_SQRT2 * (double)q[3];
 248
 249         qda = q0 * q1;
 250         qdb = q0 * q2;
 251         qdc = q0 * q3;
 252         qaa = q1 * q1;
 253         qab = q1 * q2;
 254         qac = q1 * q3;
 255         qbb = q2 * q2;
 256         qbc = q2 * q3;
 257         qcc = q3 * q3;
 258
 259         m[0][0] = (float)(1.0 - qbb - qcc);
 260         m[0][1] = (float)(qdc + qab);
 261         m[0][2] = (float)(-qdb + qac);
 262         m[0][3] = 0.0f;
 263
 264         m[1][0] = (float)(-qdc + qab);
 265         m[1][1] = (float)(1.0 - qaa - qcc);
 266         m[1][2] = (float)(qda + qbc);
 267         m[1][3] = 0.0f;
 268
 269         m[2][0] = (float)(qdb + qac);
 270         m[2][1] = (float)(-qda + qbc);
 271         m[2][2] = (float)(1.0 - qaa - qbb);
 272         m[2][3] = 0.0f;
 273
 274         m[3][0] = m[3][1] = m[3][2] = 0.0f;
 275         m[3][3] = 1.0f;
 276 }
 277
 278 void mat3_to_quat(float q[4], float wmat[3][3])
 279 {
 280         double tr, s;
 281         float mat[3][3];
 282
 283         /* work on a copy */
 284         copy_m3_m3(mat, wmat);
 285         normalize_m3(mat); /* this is needed AND a 'normalize_qt' in the end */
 286
 287         tr = 0.25 * (double)(1.0f + mat[0][0] + mat[1][1] + mat[2][2]);
 288
 289         if (tr > (double)1e-4f) {
 290                 s = sqrt(tr);
 291                 q[0] = (float)s;
 292                 s = 1.0 / (4.0 * s);
 293                 q[1] = (float)((double)(mat[1][2] - mat[2][1]) * s);
 294                 q[2] = (float)((double)(mat[2][0] - mat[0][2]) * s);
 295                 q[3] = (float)((double)(mat[0][1] - mat[1][0]) * s);
 296         }
 297         else {
 298                 if (mat[0][0] > mat[1][1] && mat[0][0] > mat[2][2]) {
 299                         s = 2.0f * sqrtf(1.0f + mat[0][0] - mat[1][1] - mat[2][2]);
 300                         q[1] = (float)(0.25 * s);
 301
 302                         s = 1.0 / s;
 303                         q[0] = (float)((double)(mat[1][2] - mat[2][1]) * s);
 304                         q[2] = (float)((double)(mat[1][0] + mat[0][1]) * s);
 305                         q[3] = (float)((double)(mat[2][0] + mat[0][2]) * s);
 306                 }
 307                 else if (mat[1][1] > mat[2][2]) {
 308                         s = 2.0f * sqrtf(1.0f + mat[1][1] - mat[0][0] - mat[2][2]);
 309                         q[2] = (float)(0.25 * s);
 310
 311                         s = 1.0 / s;
 312                         q[0] = (float)((double)(mat[2][0] - mat[0][2]) * s);
 313                         q[1] = (float)((double)(mat[1][0] + mat[0][1]) * s);
 314                         q[3] = (float)((double)(mat[2][1] + mat[1][2]) * s);
 315                 }
 316                 else {
 317                         s = 2.0f * sqrtf(1.0f + mat[2][2] - mat[0][0] - mat[1][1]);
 318                         q[3] = (float)(0.25 * s);
 319
 320                         s = 1.0 / s;
 321                         q[0] = (float)((double)(mat[0][1] - mat[1][0]) * s);
 322                         q[1] = (float)((double)(mat[2][0] + mat[0][2]) * s);
 323                         q[2] = (float)((double)(mat[2][1] + mat[1][2]) * s);
 324                 }
 325         }
 326
 327         normalize_qt(q);
 328 }
 329
 330 void mat4_to_quat(float q[4], float m[4][4])
 331 {
 332         float mat[3][3];
 333
 334         copy_m3_m4(mat, m);
 335         mat3_to_quat(q, mat);
 336 }
 337
 338 void mat3_to_quat_is_ok(float q[4], float wmat[3][3])
 339 {
 340         float mat[3][3], matr[3][3], matn[3][3], q1[4], q2[4], angle, si, co, nor[3];
 341
 342         /* work on a copy */
 343         copy_m3_m3(mat, wmat);
 344         normalize_m3(mat);
 345
 346         /* rotate z-axis of matrix to z-axis */
 347
 348         nor[0] = mat[2][1]; /* cross product with (0,0,1) */
 349         nor[1] = -mat[2][0];
 350         nor[2] = 0.0;
 351         normalize_v3(nor);
 352
 353         co = mat[2][2];
 354         angle = 0.5f * saacos(co);
 355
 356         co = cosf(angle);
 357         si = sinf(angle);
 358         q1[0] = co;
 359         q1[1] = -nor[0] * si; /* negative here, but why? */
 360         q1[2] = -nor[1] * si;
 361         q1[3] = -nor[2] * si;
 362
 363         /* rotate back x-axis from mat, using inverse q1 */
 364         quat_to_mat3_no_error(matr, q1);
 365         invert_m3_m3(matn, matr);
 366         mul_m3_v3(matn, mat[0]);
 367
 368         /* and align x-axes */
 369         angle = (float)(0.5 * atan2(mat[0][1], mat[0][0]));
 370
 371         co = cosf(angle);
 372         si = sinf(angle);
 373         q2[0] = co;
 374         q2[1] = 0.0f;
 375         q2[2] = 0.0f;
 376         q2[3] = si;
 377
 378         mul_qt_qtqt(q, q1, q2);
 379 }
 380
 381 float normalize_qt(float q[4])
 382 {
 383         float len;
 384
 385         len = sqrtf(dot_qtqt(q, q));
 386         if (len != 0.0f) {
 387                 mul_qt_fl(q, 1.0f / len);
 388         }
 389         else {
 390                 q[1] = 1.0f;
 391                 q[0] = q[2] = q[3] = 0.0f;
 392         }
 393
 394         return len;
 395 }
 396
 397 float normalize_qt_qt(float r[4], const float q[4])
 398 {
 399         copy_qt_qt(r, q);
 400         return normalize_qt(r);
 401 }
 402
 403 /**
 404  * Calculate a rotation matrix from 2 normalized vectors.
 405  */
 406 void rotation_between_vecs_to_mat3(float m[3][3], const float v1[3], const float v2[3])
 407 {
 408         float axis[3];
 409         /* avoid calculating the angle */
 410         float angle_sin;
 411         float angle_cos;
 412
 413         BLI_ASSERT_UNIT_V3(v1);
 414         BLI_ASSERT_UNIT_V3(v2);
 415
 416         cross_v3_v3v3(axis, v1, v2);
 417
 418         angle_sin = normalize_v3(axis);
 419         angle_cos = dot_v3v3(v1, v2);
 420
 421         if (angle_sin > FLT_EPSILON) {
 422 axis_calc:
 423                 BLI_ASSERT_UNIT_V3(axis);
 424                 axis_angle_normalized_to_mat3_ex(m, axis, angle_sin, angle_cos);
 425                 BLI_ASSERT_UNIT_M3(m);
 426         }
 427         else {
 428                 if (angle_cos > 0.0f) {
 429                         /* Same vectors, zero rotation... */
 430                         unit_m3(m);
 431                 }
 432                 else {
 433                         /* Colinear but opposed vectors, 180 rotation... */
 434                         ortho_v3_v3(axis, v1);
 435                         normalize_v3(axis);
 436                         angle_sin =  0.0f;  /* sin(M_PI) */
 437                         angle_cos = -1.0f;  /* cos(M_PI) */
 438                         goto axis_calc;
 439                 }
 440         }
 441 }
 442
 443 /* note: expects vectors to be normalized */
 444 void rotation_between_vecs_to_quat(float q[4], const float v1[3], const float v2[3])
 445 {
 446         float axis[3];
 447
 448         cross_v3_v3v3(axis, v1, v2);
 449
 450         if (normalize_v3(axis) > FLT_EPSILON) {
 451                 float angle;
 452
 453                 angle = angle_normalized_v3v3(v1, v2);
 454
 455                 axis_angle_normalized_to_quat(q, axis, angle);
 456         }
 457         else {
 458                 /* degenerate case */
 459
 460                 if (dot_v3v3(v1, v2) > 0.0f) {
 461                         /* Same vectors, zero rotation... */
 462                         unit_qt(q);
 463                 }
 464                 else {
 465                         /* Colinear but opposed vectors, 180 rotation... */
 466                         ortho_v3_v3(axis, v1);
 467                         axis_angle_to_quat(q, axis, (float)M_PI);
 468                 }
 469         }
 470 }
 471
 472 void rotation_between_quats_to_quat(float q[4], const float q1[4], const float q2[4])
 473 {
 474         float tquat[4];
 475
 476         conjugate_qt_qt(tquat, q1);
 477
 478         mul_qt_fl(tquat, 1.0f / dot_qtqt(tquat, tquat));
 479
 480         mul_qt_qtqt(q, tquat, q2);
 481 }
 482
 483
 484 float angle_normalized_qt(const float q[4])
 485 {
 486         BLI_ASSERT_UNIT_QUAT(q);
 487         return 2.0f * saacos(q[0]);
 488 }
 489
 490 float angle_qt(const float q[4])
 491 {
 492         float tquat[4];
 493
 494         normalize_qt_qt(tquat, q);
 495
 496         return angle_normalized_qt(tquat);
 497 }
 498
 499 float angle_normalized_qtqt(const float q1[4], const float q2[4])
 500 {
 501         float qdelta[4];
 502
 503         BLI_ASSERT_UNIT_QUAT(q1);
 504         BLI_ASSERT_UNIT_QUAT(q2);
 505
 506         rotation_between_quats_to_quat(qdelta, q1, q2);
 507
 508         return angle_normalized_qt(qdelta);
 509 }
 510
 511 float angle_qtqt(const float q1[4], const float q2[4])
 512 {
 513         float quat1[4], quat2[4];
 514
 515         normalize_qt_qt(quat1, q1);
 516         normalize_qt_qt(quat2, q2);
 517
 518         return angle_normalized_qtqt(quat1, quat2);
 519 }
 520
 521 void vec_to_quat(float q[4], const float vec[3], short axis, const short upflag)
 522 {
 523         const float eps = 0.0001f;
 524         float nor[3], tvec[3];
 525         float angle, si, co, len;
 526
 527         assert(axis >= 0 && axis <= 5);
 528         assert(upflag >= 0 && upflag <= 2);
 529
 530         /* first set the quat to unit */
 531         unit_qt(q);
 532
 533         len = len_v3(vec);
 534
 535         if (UNLIKELY(len == 0.0f)) {
 536                 return;
 537         }
 538
 539         /* rotate to axis */
 540         if (axis > 2) {
 541                 copy_v3_v3(tvec, vec);
 542                 axis = (short)(axis - 3);
 543         }
 544         else {
 545                 negate_v3_v3(tvec, vec);
 546         }
 547
 548         /* nasty! I need a good routine for this...
 549          * problem is a rotation of an Y axis to the negative Y-axis for example.
 550          */
 551
 552         if (axis == 0) { /* x-axis */
 553                 nor[0] =  0.0;
 554                 nor[1] = -tvec[2];
 555                 nor[2] =  tvec[1];
 556
 557                 if (fabsf(tvec[1]) + fabsf(tvec[2]) < eps)
 558                         nor[1] = 1.0f;
 559
 560                 co = tvec[0];
 561         }
 562         else if (axis == 1) { /* y-axis */
 563                 nor[0] =  tvec[2];
 564                 nor[1] =  0.0;
 565                 nor[2] = -tvec[0];
 566
 567                 if (fabsf(tvec[0]) + fabsf(tvec[2]) < eps)
 568                         nor[2] = 1.0f;
 569
 570                 co = tvec[1];
 571         }
 572         else { /* z-axis */
 573                 nor[0] = -tvec[1];
 574                 nor[1] =  tvec[0];
 575                 nor[2] =  0.0;
 576
 577                 if (fabsf(tvec[0]) + fabsf(tvec[1]) < eps)
 578                         nor[0] = 1.0f;
 579
 580                 co = tvec[2];
 581         }
 582         co /= len;
 583
 584         normalize_v3(nor);
 585
 586         axis_angle_normalized_to_quat(q, nor, saacos(co));
 587
 588         if (axis != upflag) {
 589                 float mat[3][3];
 590                 float q2[4];
 591                 const float *fp = mat[2];
 592                 quat_to_mat3(mat, q);
 593
 594                 if (axis == 0) {
 595                         if (upflag == 1) angle =  0.5f * atan2f(fp[2], fp[1]);
 596                         else             angle = -0.5f * atan2f(fp[1], fp[2]);
 597                 }
 598                 else if (axis == 1) {
 599                         if (upflag == 0) angle = -0.5f * atan2f(fp[2], fp[0]);
 600                         else             angle =  0.5f * atan2f(fp[0], fp[2]);
 601                 }
 602                 else {
 603                         if (upflag == 0) angle =  0.5f * atan2f(-fp[1], -fp[0]);
 604                         else             angle = -0.5f * atan2f(-fp[0], -fp[1]);
 605                 }
 606
 607                 co = cosf(angle);
 608                 si = sinf(angle) / len;
 609                 q2[0] = co;
 610                 q2[1] = tvec[0] * si;
 611                 q2[2] = tvec[1] * si;
 612                 q2[3] = tvec[2] * si;
 613
 614                 mul_qt_qtqt(q, q2, q);
 615         }
 616 }
 617
 618 #if 0
 619
 620 /* A & M Watt, Advanced animation and rendering techniques, 1992 ACM press */
 621 void QuatInterpolW(float *result, float quat1[4], float quat2[4], float t)
 622 {
 623         float omega, cosom, sinom, sc1, sc2;
 624
 625         cosom = quat1[0] * quat2[0] + quat1[1] * quat2[1] + quat1[2] * quat2[2] + quat1[3] * quat2[3];
 626
 627         /* rotate around shortest angle */
 628         if ((1.0f + cosom) > 0.0001f) {
 629
 630                 if ((1.0f - cosom) > 0.0001f) {
 631                         omega = (float)acos(cosom);
 632                         sinom = sinf(omega);
 633                         sc1 = sinf((1.0 - t) * omega) / sinom;
 634                         sc2 = sinf(t * omega) / sinom;
 635                 }
 636                 else {
 637                         sc1 = 1.0f - t;
 638                         sc2 = t;
 639                 }
 640                 result[0] = sc1 * quat1[0] + sc2 * quat2[0];
 641                 result[1] = sc1 * quat1[1] + sc2 * quat2[1];
 642                 result[2] = sc1 * quat1[2] + sc2 * quat2[2];
 643                 result[3] = sc1 * quat1[3] + sc2 * quat2[3];
 644         }
 645         else {
 646                 result[0] = quat2[3];
 647                 result[1] = -quat2[2];
 648                 result[2] = quat2[1];
 649                 result[3] = -quat2[0];
 650
 651                 sc1 = sinf((1.0 - t) * M_PI_2);
 652                 sc2 = sinf(t * M_PI_2);
 653
 654                 result[0] = sc1 * quat1[0] + sc2 * result[0];
 655                 result[1] = sc1 * quat1[1] + sc2 * result[1];
 656                 result[2] = sc1 * quat1[2] + sc2 * result[2];
 657                 result[3] = sc1 * quat1[3] + sc2 * result[3];
 658         }
 659 }
 660 #endif
 661
 662 /**
 663  * Generic function for implementing slerp
 664  * (quaternions and spherical vector coords).
 665  *
 666  * \param t: factor in [0..1]
 667  * \param cosom: dot product from normalized vectors/quats.
 668  * \param r_w: calculated weights.
 669  */
 670 void interp_dot_slerp(const float t, const float cosom, float r_w[2])
 671 {
 672         const float eps = 0.0001f;
 673
 674         BLI_assert(IN_RANGE_INCL(cosom, -1.0001f, 1.0001f));
 675
 676         /* within [-1..1] range, avoid aligned axis */
 677         if (LIKELY(fabsf(cosom) < (1.0f - eps))) {
 678                 float omega, sinom;
 679
 680                 omega = acosf(cosom);
 681                 sinom = sinf(omega);
 682                 r_w[0] = sinf((1.0f - t) * omega) / sinom;
 683                 r_w[1] = sinf(t * omega) / sinom;
 684         }
 685         else {
 686                 /* fallback to lerp */
 687                 r_w[0] = 1.0f - t;
 688                 r_w[1] = t;
 689         }
 690 }
 691
 692 void interp_qt_qtqt(float result[4], const float quat1[4], const float quat2[4], const float t)
 693 {
 694         float quat[4], cosom, w[2];
 695
 696         BLI_ASSERT_UNIT_QUAT(quat1);
 697         BLI_ASSERT_UNIT_QUAT(quat2);
 698
 699         cosom = dot_qtqt(quat1, quat2);
 700
 701         /* rotate around shortest angle */
 702         if (cosom < 0.0f) {
 703                 cosom = -cosom;
 704                 negate_v4_v4(quat, quat1);
 705         }
 706         else {
 707                 copy_qt_qt(quat, quat1);
 708         }
 709
 710         interp_dot_slerp(t, cosom, w);
 711
 712         result[0] = w[0] * quat[0] + w[1] * quat2[0];
 713         result[1] = w[0] * quat[1] + w[1] * quat2[1];
 714         result[2] = w[0] * quat[2] + w[1] * quat2[2];
 715         result[3] = w[0] * quat[3] + w[1] * quat2[3];
 716 }
 717
 718 void add_qt_qtqt(float result[4], const float quat1[4], const float quat2[4], const float t)
 719 {
 720         result[0] = quat1[0] + t * quat2[0];
 721         result[1] = quat1[1] + t * quat2[1];
 722         result[2] = quat1[2] + t * quat2[2];
 723         result[3] = quat1[3] + t * quat2[3];
 724 }
 725
 726 /* same as tri_to_quat() but takes pre-computed normal from the triangle
 727  * used for ngons when we know their normal */
 728 void tri_to_quat_ex(float quat[4], const float v1[3], const float v2[3], const float v3[3],
 729                     const float no_orig[3])
 730 {
 731         /* imaginary x-axis, y-axis triangle is being rotated */
 732         float vec[3], q1[4], q2[4], n[3], si, co, angle, mat[3][3], imat[3][3];
 733
 734         /* move z-axis to face-normal */
 735 #if 0
 736         normal_tri_v3(vec, v1, v2, v3);
 737 #else
 738         copy_v3_v3(vec, no_orig);
 739         (void)v3;
 740 #endif
 741
 742         n[0] =  vec[1];
 743         n[1] = -vec[0];
 744         n[2] =  0.0f;
 745         normalize_v3(n);
 746
 747         if (n[0] == 0.0f && n[1] == 0.0f) {
 748                 n[0] = 1.0f;
 749         }
 750
 751         angle = -0.5f * saacos(vec[2]);
 752         co = cosf(angle);
 753         si = sinf(angle);
 754         q1[0] = co;
 755         q1[1] = n[0] * si;
 756         q1[2] = n[1] * si;
 757         q1[3] = 0.0f;
 758
 759         /* rotate back line v1-v2 */
 760         quat_to_mat3(mat, q1);
 761         invert_m3_m3(imat, mat);
 762         sub_v3_v3v3(vec, v2, v1);
 763         mul_m3_v3(imat, vec);
 764
 765         /* what angle has this line with x-axis? */
 766         vec[2] = 0.0f;
 767         normalize_v3(vec);
 768
 769         angle = (float)(0.5 * atan2(vec[1], vec[0]));
 770         co = cosf(angle);
 771         si = sinf(angle);
 772         q2[0] = co;
 773         q2[1] = 0.0f;
 774         q2[2] = 0.0f;
 775         q2[3] = si;
 776
 777         mul_qt_qtqt(quat, q1, q2);
 778 }
 779
 780 /**
 781  * \return the length of the normal, use to test for degenerate triangles.
 782  */
 783 float tri_to_quat(float quat[4], const float v1[3], const float v2[3], const float v3[3])
 784 {
 785         float vec[3];
 786         float len;
 787
 788         len = normal_tri_v3(vec, v1, v2, v3);
 789         tri_to_quat_ex(quat, v1, v2, v3, vec);
 790         return len;
 791 }
 792
 793 void print_qt(const char *str, const float q[4])
 794 {
 795         printf("%s: %.3f %.3f %.3f %.3f\n", str, q[0], q[1], q[2], q[3]);
 796 }
 797
 798 /******************************** Axis Angle *********************************/
 799
 800 void axis_angle_normalized_to_quat(float q[4], const float axis[3], const float angle)
 801 {
 802         const float phi = 0.5f * angle;
 803         const float si = sinf(phi);
 804         const float co = cosf(phi);
 805         BLI_ASSERT_UNIT_V3(axis);
 806         q[0] = co;
 807         mul_v3_v3fl(q + 1, axis, si);
 808 }
 809
 810 void axis_angle_to_quat(float q[4], const float axis[3], const float angle)
 811 {
 812         float nor[3];
 813
 814         if (LIKELY(normalize_v3_v3(nor, axis) != 0.0f)) {
 815                 axis_angle_normalized_to_quat(q, nor, angle);
 816         }
 817         else {
 818                 unit_qt(q);
 819         }
 820 }
 821
 822 /* Quaternions to Axis Angle */
 823 void quat_to_axis_angle(float axis[3], float *angle, const float q[4])
 824 {
 825         float ha, si;
 826
 827 #ifdef DEBUG
 828         if (!((ha = dot_qtqt(q, q)) == 0.0f || (fabsf(ha - 1.0f) < (float)QUAT_EPSILON))) {
 829                 fprintf(stderr, "Warning! quat_to_axis_angle() called with non-normalized: size %.8f *** report a bug ***\n", ha);
 830         }
 831 #endif
 832
 833         /* calculate angle/2, and sin(angle/2) */
 834         ha = acosf(q[0]);
 835         si = sinf(ha);
 836
 837         /* from half-angle to angle */
 838         *angle = ha * 2;
 839
 840         /* prevent division by zero for axis conversion */
 841         if (fabsf(si) < 0.0005f)
 842                 si = 1.0f;
 843
 844         axis[0] = q[1] / si;
 845         axis[1] = q[2] / si;
 846         axis[2] = q[3] / si;
 847 }
 848
 849 /* Axis Angle to Euler Rotation */
 850 void axis_angle_to_eulO(float eul[3], const short order, const float axis[3], const float angle)
 851 {
 852         float q[4];
 853
 854         /* use quaternions as intermediate representation for now... */
 855         axis_angle_to_quat(q, axis, angle);
 856         quat_to_eulO(eul, order, q);
 857 }
 858
 859 /* Euler Rotation to Axis Angle */
 860 void eulO_to_axis_angle(float axis[3], float *angle, const float eul[3], const short order)
 861 {
 862         float q[4];
 863
 864         /* use quaternions as intermediate representation for now... */
 865         eulO_to_quat(q, eul, order);
 866         quat_to_axis_angle(axis, angle, q);
 867 }
 868
 869 /**
 870  * axis angle to 3x3 matrix
 871  *
 872  * This takes the angle with sin/cos applied so we can avoid calculating it in some cases.
 873  *
 874  * \param axis rotation axis (must be normalized).
 875  * \param angle_sin sin(angle)
 876  * \param angle_cos cos(angle)
 877  */
 878 void axis_angle_normalized_to_mat3_ex(float mat[3][3], const float axis[3],
 879                                       const float angle_sin, const float angle_cos)
 880 {
 881         float nsi[3], ico;
 882         float n_00, n_01, n_11, n_02, n_12, n_22;
 883
 884         BLI_ASSERT_UNIT_V3(axis);
 885
 886         /* now convert this to a 3x3 matrix */
 887
 888         ico = (1.0f - angle_cos);
 889         nsi[0] = axis[0] * angle_sin;
 890         nsi[1] = axis[1] * angle_sin;
 891         nsi[2] = axis[2] * angle_sin;
 892
 893         n_00 = (axis[0] * axis[0]) * ico;
 894         n_01 = (axis[0] * axis[1]) * ico;
 895         n_11 = (axis[1] * axis[1]) * ico;
 896         n_02 = (axis[0] * axis[2]) * ico;
 897         n_12 = (axis[1] * axis[2]) * ico;
 898         n_22 = (axis[2] * axis[2]) * ico;
 899
 900         mat[0][0] = n_00 + angle_cos;
 901         mat[0][1] = n_01 + nsi[2];
 902         mat[0][2] = n_02 - nsi[1];
 903         mat[1][0] = n_01 - nsi[2];
 904         mat[1][1] = n_11 + angle_cos;
 905         mat[1][2] = n_12 + nsi[0];
 906         mat[2][0] = n_02 + nsi[1];
 907         mat[2][1] = n_12 - nsi[0];
 908         mat[2][2] = n_22 + angle_cos;
 909 }
 910
 911 void axis_angle_normalized_to_mat3(float mat[3][3], const float axis[3], const float angle)
 912 {
 913         axis_angle_normalized_to_mat3_ex(mat, axis, sinf(angle), cosf(angle));
 914 }
 915
 916
 917 /* axis angle to 3x3 matrix - safer version (normalization of axis performed) */
 918 void axis_angle_to_mat3(float mat[3][3], const float axis[3], const float angle)
 919 {
 920         float nor[3];
 921
 922         /* normalize the axis first (to remove unwanted scaling) */
 923         if (normalize_v3_v3(nor, axis) == 0.0f) {
 924                 unit_m3(mat);
 925                 return;
 926         }
 927
 928         axis_angle_normalized_to_mat3(mat, nor, angle);
 929 }
 930
 931 /* axis angle to 4x4 matrix - safer version (normalization of axis performed) */
 932 void axis_angle_to_mat4(float mat[4][4], const float axis[3], const float angle)
 933 {
 934         float tmat[3][3];
 935
 936         axis_angle_to_mat3(tmat, axis, angle);
 937         unit_m4(mat);
 938         copy_m4_m3(mat, tmat);
 939 }
 940
 941 /* 3x3 matrix to axis angle (see Mat4ToVecRot too) */
 942 void mat3_to_axis_angle(float axis[3], float *angle, float mat[3][3])
 943 {
 944         float q[4];
 945
 946         /* use quaternions as intermediate representation */
 947         /* TODO: it would be nicer to go straight there... */
 948         mat3_to_quat(q, mat);
 949         quat_to_axis_angle(axis, angle, q);
 950 }
 951
 952 /* 4x4 matrix to axis angle (see Mat4ToVecRot too) */
 953 void mat4_to_axis_angle(float axis[3], float *angle, float mat[4][4])
 954 {
 955         float q[4];
 956
 957         /* use quaternions as intermediate representation */
 958         /* TODO: it would be nicer to go straight there... */
 959         mat4_to_quat(q, mat);
 960         quat_to_axis_angle(axis, angle, q);
 961 }
 962
 963 /* rotation matrix from a single axis */
 964 void axis_angle_to_mat3_single(float mat[3][3], const char axis, const float angle)
 965 {
 966         const float angle_cos = cosf(angle);
 967         const float angle_sin = sinf(angle);
 968
 969         switch (axis) {
 970                 case 'X': /* rotation around X */
 971                         mat[0][0] = 1.0f;
 972                         mat[0][1] = 0.0f;
 973                         mat[0][2] = 0.0f;
 974                         mat[1][0] = 0.0f;
 975                         mat[1][1] = angle_cos;
 976                         mat[1][2] = angle_sin;
 977                         mat[2][0] = 0.0f;
 978                         mat[2][1] = -angle_sin;
 979                         mat[2][2] = angle_cos;
 980                         break;
 981                 case 'Y': /* rotation around Y */
 982                         mat[0][0] = angle_cos;
 983                         mat[0][1] = 0.0f;
 984                         mat[0][2] = -angle_sin;
 985                         mat[1][0] = 0.0f;
 986                         mat[1][1] = 1.0f;
 987                         mat[1][2] = 0.0f;
 988                         mat[2][0] = angle_sin;
 989                         mat[2][1] = 0.0f;
 990                         mat[2][2] = angle_cos;
 991                         break;
 992                 case 'Z': /* rotation around Z */
 993                         mat[0][0] = angle_cos;
 994                         mat[0][1] = angle_sin;
 995                         mat[0][2] = 0.0f;
 996                         mat[1][0] = -angle_sin;
 997                         mat[1][1] = angle_cos;
 998                         mat[1][2] = 0.0f;
 999                         mat[2][0] = 0.0f;
1000                         mat[2][1] = 0.0f;
1001                         mat[2][2] = 1.0f;
1002                         break;
1003                 default:
1004                         BLI_assert(0);
1005                         break;
1006         }
1007 }
1008
1009 void angle_to_mat2(float mat[2][2], const float angle)
1010 {
1011         const float angle_cos = cosf(angle);
1012         const float angle_sin = sinf(angle);
1013
1014         /* 2D rotation matrix */
1015         mat[0][0] =  angle_cos;
1016         mat[0][1] =  angle_sin;
1017         mat[1][0] = -angle_sin;
1018         mat[1][1] =  angle_cos;
1019 }
1020
1021 /******************************** XYZ Eulers *********************************/
1022
1023 /* XYZ order */
1024 void eul_to_mat3(float mat[3][3], const float eul[3])
1025 {
1026         double ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
1027
1028         ci = cos(eul[0]);
1029         cj = cos(eul[1]);
1030         ch = cos(eul[2]);
1031         si = sin(eul[0]);
1032         sj = sin(eul[1]);
1033         sh = sin(eul[2]);
1034         cc = ci * ch;
1035         cs = ci * sh;
1036         sc = si * ch;
1037         ss = si * sh;
1038
1039         mat[0][0] = (float)(cj * ch);
1040         mat[1][0] = (float)(sj * sc - cs);
1041         mat[2][0] = (float)(sj * cc + ss);
1042         mat[0][1] = (float)(cj * sh);
1043         mat[1][1] = (float)(sj * ss + cc);
1044         mat[2][1] = (float)(sj * cs - sc);
1045         mat[0][2] = (float)-sj;
1046         mat[1][2] = (float)(cj * si);
1047         mat[2][2] = (float)(cj * ci);
1048
1049 }
1050
1051 /* XYZ order */
1052 void eul_to_mat4(float mat[4][4], const float eul[3])
1053 {
1054         double ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
1055
1056         ci = cos(eul[0]);
1057         cj = cos(eul[1]);
1058         ch = cos(eul[2]);
1059         si = sin(eul[0]);
1060         sj = sin(eul[1]);
1061         sh = sin(eul[2]);
1062         cc = ci * ch;
1063         cs = ci * sh;
1064         sc = si * ch;
1065         ss = si * sh;
1066
1067         mat[0][0] = (float)(cj * ch);
1068         mat[1][0] = (float)(sj * sc - cs);
1069         mat[2][0] = (float)(sj * cc + ss);
1070         mat[0][1] = (float)(cj * sh);
1071         mat[1][1] = (float)(sj * ss + cc);
1072         mat[2][1] = (float)(sj * cs - sc);
1073         mat[0][2] = (float)-sj;
1074         mat[1][2] = (float)(cj * si);
1075         mat[2][2] = (float)(cj * ci);
1076
1077
1078         mat[3][0] = mat[3][1] = mat[3][2] = mat[0][3] = mat[1][3] = mat[2][3] = 0.0f;
1079         mat[3][3] = 1.0f;
1080 }
1081
1082 /* returns two euler calculation methods, so we can pick the best */
1083
1084 /* XYZ order */
1085 static void mat3_to_eul2(float tmat[3][3], float eul1[3], float eul2[3])
1086 {
1087         float cy, quat[4], mat[3][3];
1088
1089         mat3_to_quat(quat, tmat);
1090         quat_to_mat3(mat, quat);
1091         normalize_m3_m3(mat, tmat);
1092
1093         cy = hypotf(mat[0][0], mat[0][1]);
1094
1095         if (cy > 16.0f * FLT_EPSILON) {
1096
1097                 eul1[0] = atan2f(mat[1][2], mat[2][2]);
1098                 eul1[1] = atan2f(-mat[0][2], cy);
1099                 eul1[2] = atan2f(mat[0][1], mat[0][0]);
1100
1101                 eul2[0] = atan2f(-mat[1][2], -mat[2][2]);
1102                 eul2[1] = atan2f(-mat[0][2], -cy);
1103                 eul2[2] = atan2f(-mat[0][1], -mat[0][0]);
1104
1105         }
1106         else {
1107                 eul1[0] = atan2f(-mat[2][1], mat[1][1]);
1108                 eul1[1] = atan2f(-mat[0][2], cy);
1109                 eul1[2] = 0.0f;
1110
1111                 copy_v3_v3(eul2, eul1);
1112         }
1113 }
1114
1115 /* XYZ order */
1116 void mat3_to_eul(float *eul, float tmat[3][3])
1117 {
1118         float eul1[3], eul2[3];
1119
1120         mat3_to_eul2(tmat, eul1, eul2);
1121
1122         /* return best, which is just the one with lowest values it in */
1123         if (fabsf(eul1[0]) + fabsf(eul1[1]) + fabsf(eul1[2]) > fabsf(eul2[0]) + fabsf(eul2[1]) + fabsf(eul2[2])) {
1124                 copy_v3_v3(eul, eul2);
1125         }
1126         else {
1127                 copy_v3_v3(eul, eul1);
1128         }
1129 }
1130
1131 /* XYZ order */
1132 void mat4_to_eul(float *eul, float tmat[4][4])
1133 {
1134         float tempMat[3][3];
1135
1136         copy_m3_m4(tempMat, tmat);
1137         normalize_m3(tempMat);
1138         mat3_to_eul(eul, tempMat);
1139 }
1140
1141 /* XYZ order */
1142 void quat_to_eul(float *eul, const float quat[4])
1143 {
1144         float mat[3][3];
1145
1146         quat_to_mat3(mat, quat);
1147         mat3_to_eul(eul, mat);
1148 }
1149
1150 /* XYZ order */
1151 void eul_to_quat(float quat[4], const float eul[3])
1152 {
1153         float ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
1154
1155         ti = eul[0] * 0.5f;
1156         tj = eul[1] * 0.5f;
1157         th = eul[2] * 0.5f;
1158         ci = cosf(ti);
1159         cj = cosf(tj);
1160         ch = cosf(th);
1161         si = sinf(ti);
1162         sj = sinf(tj);
1163         sh = sinf(th);
1164         cc = ci * ch;
1165         cs = ci * sh;
1166         sc = si * ch;
1167         ss = si * sh;
1168
1169         quat[0] = cj * cc + sj * ss;
1170         quat[1] = cj * sc - sj * cs;
1171         quat[2] = cj * ss + sj * cc;
1172         quat[3] = cj * cs - sj * sc;
1173 }
1174
1175 /* XYZ order */
1176 void rotate_eul(float beul[3], const char axis, const float ang)
1177 {
1178         float eul[3], mat1[3][3], mat2[3][3], totmat[3][3];
1179
1180         assert(axis >= 'X' && axis <= 'Z');
1181
1182         eul[0] = eul[1] = eul[2] = 0.0f;
1183         if (axis == 'X') eul[0] = ang;
1184         else if (axis == 'Y') eul[1] = ang;
1185         else eul[2] = ang;
1186
1187         eul_to_mat3(mat1, eul);
1188         eul_to_mat3(mat2, beul);
1189
1190         mul_m3_m3m3(totmat, mat2, mat1);
1191
1192         mat3_to_eul(beul, totmat);
1193 }
1194
1195 /* order independent! */
1196 void compatible_eul(float eul[3], const float oldrot[3])
1197 {
1198         /* we could use M_PI as pi_thresh: which is correct but 5.1 gives better results.
1199          * Checked with baking actions to fcurves - campbell */
1200         const float pi_thresh = (5.1f);
1201         const float pi_x2     = (2.0f * (float)M_PI);
1202
1203         float deul[3];
1204         unsigned int i;
1205
1206         /* correct differences of about 360 degrees first */
1207         for (i = 0; i < 3; i++) {
1208                 deul[i] = eul[i] - oldrot[i];
1209                 if (deul[i] > pi_thresh) {
1210                         eul[i] -= floorf(( deul[i] / pi_x2) + 0.5f) * pi_x2;
1211                         deul[i] = eul[i] - oldrot[i];
1212                 }
1213                 else if (deul[i] < -pi_thresh) {
1214                         eul[i] += floorf((-deul[i] / pi_x2) + 0.5f) * pi_x2;
1215                         deul[i] = eul[i] - oldrot[i];
1216                 }
1217         }
1218
1219         /* is 1 of the axis rotations larger than 180 degrees and the other small? NO ELSE IF!! */
1220         if (fabsf(deul[0]) > 3.2f && fabsf(deul[1]) < 1.6f && fabsf(deul[2]) < 1.6f) {
1221                 if (deul[0] > 0.0f) eul[0] -= pi_x2;
1222                 else                eul[0] += pi_x2;
1223         }
1224         if (fabsf(deul[1]) > 3.2f && fabsf(deul[2]) < 1.6f && fabsf(deul[0]) < 1.6f) {
1225                 if (deul[1] > 0.0f) eul[1] -= pi_x2;
1226                 else                eul[1] += pi_x2;
1227         }
1228         if (fabsf(deul[2]) > 3.2f && fabsf(deul[0]) < 1.6f && fabsf(deul[1]) < 1.6f) {
1229                 if (deul[2] > 0.0f) eul[2] -= pi_x2;
1230                 else                eul[2] += pi_x2;
1231         }
1232 }
1233
1234 /* uses 2 methods to retrieve eulers, and picks the closest */
1235
1236 /* XYZ order */
1237 void mat3_to_compatible_eul(float eul[3], const float oldrot[3], float mat[3][3])
1238 {
1239         float eul1[3], eul2[3];
1240         float d1, d2;
1241
1242         mat3_to_eul2(mat, eul1, eul2);
1243
1244         compatible_eul(eul1, oldrot);
1245         compatible_eul(eul2, oldrot);
1246
1247         d1 = fabsf(eul1[0] - oldrot[0]) + fabsf(eul1[1] - oldrot[1]) + fabsf(eul1[2] - oldrot[2]);
1248         d2 = fabsf(eul2[0] - oldrot[0]) + fabsf(eul2[1] - oldrot[1]) + fabsf(eul2[2] - oldrot[2]);
1249
1250         /* return best, which is just the one with lowest difference */
1251         if (d1 > d2) {
1252                 copy_v3_v3(eul, eul2);
1253         }
1254         else {
1255                 copy_v3_v3(eul, eul1);
1256         }
1257 }
1258
1259 /************************** Arbitrary Order Eulers ***************************/
1260
1261 /* Euler Rotation Order Code:
1262  * was adapted from
1263  *      ANSI C code from the article
1264  *      "Euler Angle Conversion"
1265  *      by Ken Shoemake, shoemake@graphics.cis.upenn.edu
1266  *      in "Graphics Gems IV", Academic Press, 1994
1267  * for use in Blender
1268  */
1269
1270 /* Type for rotation order info - see wiki for derivation details */
1271 typedef struct RotOrderInfo {
1272         short axis[3];
1273         short parity; /* parity of axis permutation (even=0, odd=1) - 'n' in original code */
1274 } RotOrderInfo;
1275
1276 /* Array of info for Rotation Order calculations
1277  * WARNING: must be kept in same order as eEulerRotationOrders
1278  */
1279 static const RotOrderInfo rotOrders[] = {
1280         /* i, j, k, n */
1281         {{0, 1, 2}, 0}, /* XYZ */
1282         {{0, 2, 1}, 1}, /* XZY */
1283         {{1, 0, 2}, 1}, /* YXZ */
1284         {{1, 2, 0}, 0}, /* YZX */
1285         {{2, 0, 1}, 0}, /* ZXY */
1286         {{2, 1, 0}, 1}  /* ZYX */
1287 };
1288
1289 /* Get relevant pointer to rotation order set from the array
1290  * NOTE: since we start at 1 for the values, but arrays index from 0,
1291  *               there is -1 factor involved in this process...
1292  */
1293 #define GET_ROTATIONORDER_INFO(order) (assert(order >= 0 && order <= 6), (order < 1) ? &rotOrders[0] : &rotOrders[(order) - 1])
1294
1295 /* Construct quaternion from Euler angles (in radians). */
1296 void eulO_to_quat(float q[4], const float e[3], const short order)
1297 {
1298         const RotOrderInfo *R = GET_ROTATIONORDER_INFO(order);
1299         short i = R->axis[0], j = R->axis[1], k = R->axis[2];
1300         double ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
1301         double a[3];
1302
1303         ti = e[i] * 0.5f;
1304         tj = e[j] * (R->parity ? -0.5f : 0.5f);
1305         th = e[k] * 0.5f;
1306
1307         ci = cos(ti);
1308         cj = cos(tj);
1309         ch = cos(th);
1310         si = sin(ti);
1311         sj = sin(tj);
1312         sh = sin(th);
1313
1314         cc = ci * ch;
1315         cs = ci * sh;
1316         sc = si * ch;
1317         ss = si * sh;
1318
1319         a[i] = cj * sc - sj * cs;
1320         a[j] = cj * ss + sj * cc;
1321         a[k] = cj * cs - sj * sc;
1322
1323         q[0] = (float)(cj * cc + sj * ss);
1324         q[1] = (float)(a[0]);
1325         q[2] = (float)(a[1]);
1326         q[3] = (float)(a[2]);
1327
1328         if (R->parity) q[j + 1] = -q[j + 1];
1329 }
1330
1331 /* Convert quaternion to Euler angles (in radians). */
1332 void quat_to_eulO(float e[3], short const order, const float q[4])
1333 {
1334         float M[3][3];
1335
1336         quat_to_mat3(M, q);
1337         mat3_to_eulO(e, order, M);
1338 }
1339
1340 /* Construct 3x3 matrix from Euler angles (in radians). */
1341 void eulO_to_mat3(float M[3][3], const float e[3], const short order)
1342 {
1343         const RotOrderInfo *R = GET_ROTATIONORDER_INFO(order);
1344         short i = R->axis[0], j = R->axis[1], k = R->axis[2];
1345         double ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
1346
1347         if (R->parity) {
1348                 ti = -e[i];
1349                 tj = -e[j];
1350                 th = -e[k];
1351         }
1352         else {
1353                 ti = e[i];
1354                 tj = e[j];
1355                 th = e[k];
1356         }
1357
1358         ci = cos(ti);
1359         cj = cos(tj);
1360         ch = cos(th);
1361         si = sin(ti);
1362         sj = sin(tj);
1363         sh = sin(th);
1364
1365         cc = ci * ch;
1366         cs = ci * sh;
1367         sc = si * ch;
1368         ss = si * sh;
1369
1370         M[i][i] = (float)(cj * ch);
1371         M[j][i] = (float)(sj * sc - cs);
1372         M[k][i] = (float)(sj * cc + ss);
1373         M[i][j] = (float)(cj * sh);
1374         M[j][j] = (float)(sj * ss + cc);
1375         M[k][j] = (float)(sj * cs - sc);
1376         M[i][k] = (float)(-sj);
1377         M[j][k] = (float)(cj * si);
1378         M[k][k] = (float)(cj * ci);
1379 }
1380
1381 /* returns two euler calculation methods, so we can pick the best */
1382 static void mat3_to_eulo2(float M[3][3], float eul1[3], float eul2[3], const short order)
1383 {
1384         const RotOrderInfo *R = GET_ROTATIONORDER_INFO(order);
1385         short i = R->axis[0], j = R->axis[1], k = R->axis[2];
1386         float mat[3][3];
1387         float cy;
1388
1389         /* process the matrix first */
1390         normalize_m3_m3(mat, M);
1391
1392         cy = hypotf(mat[i][i], mat[i][j]);
1393
1394         if (cy > 16.0f * FLT_EPSILON) {
1395                 eul1[i] = atan2f(mat[j][k], mat[k][k]);
1396                 eul1[j] = atan2f(-mat[i][k], cy);
1397                 eul1[k] = atan2f(mat[i][j], mat[i][i]);
1398
1399                 eul2[i] = atan2f(-mat[j][k], -mat[k][k]);
1400                 eul2[j] = atan2f(-mat[i][k], -cy);
1401                 eul2[k] = atan2f(-mat[i][j], -mat[i][i]);
1402         }
1403         else {
1404                 eul1[i] = atan2f(-mat[k][j], mat[j][j]);
1405                 eul1[j] = atan2f(-mat[i][k], cy);
1406                 eul1[k] = 0;
1407
1408                 copy_v3_v3(eul2, eul1);
1409         }
1410
1411         if (R->parity) {
1412                 negate_v3(eul1);
1413                 negate_v3(eul2);
1414         }
1415 }
1416
1417 /* Construct 4x4 matrix from Euler angles (in radians). */
1418 void eulO_to_mat4(float M[4][4], const float e[3], const short order)
1419 {
1420         float m[3][3];
1421
1422         /* for now, we'll just do this the slow way (i.e. copying matrices) */
1423         normalize_m3(m);
1424         eulO_to_mat3(m, e, order);
1425         copy_m4_m3(M, m);
1426 }
1427
1428 /* Convert 3x3 matrix to Euler angles (in radians). */
1429 void mat3_to_eulO(float eul[3], const short order, float M[3][3])
1430 {
1431         float eul1[3], eul2[3];
1432         float d1, d2;
1433
1434         mat3_to_eulo2(M, eul1, eul2, order);
1435
1436         d1 = fabsf(eul1[0]) + fabsf(eul1[1]) + fabsf(eul1[2]);
1437         d2 = fabsf(eul2[0]) + fabsf(eul2[1]) + fabsf(eul2[2]);
1438
1439         /* return best, which is just the one with lowest values it in */
1440         if (d1 > d2) {
1441                 copy_v3_v3(eul, eul2);
1442         }
1443         else {
1444                 copy_v3_v3(eul, eul1);
1445         }
1446 }
1447
1448 /* Convert 4x4 matrix to Euler angles (in radians). */
1449 void mat4_to_eulO(float e[3], const short order, float M[4][4])
1450 {
1451         float m[3][3];
1452
1453         /* for now, we'll just do this the slow way (i.e. copying matrices) */
1454         copy_m3_m4(m, M);
1455         normalize_m3(m);
1456         mat3_to_eulO(e, order, m);
1457 }
1458
1459 /* uses 2 methods to retrieve eulers, and picks the closest */
1460 void mat3_to_compatible_eulO(float eul[3], float oldrot[3], const short order, float mat[3][3])
1461 {
1462         float eul1[3], eul2[3];
1463         float d1, d2;
1464
1465         mat3_to_eulo2(mat, eul1, eul2, order);
1466
1467         compatible_eul(eul1, oldrot);
1468         compatible_eul(eul2, oldrot);
1469
1470         d1 = fabsf(eul1[0] - oldrot[0]) + fabsf(eul1[1] - oldrot[1]) + fabsf(eul1[2] - oldrot[2]);
1471         d2 = fabsf(eul2[0] - oldrot[0]) + fabsf(eul2[1] - oldrot[1]) + fabsf(eul2[2] - oldrot[2]);
1472
1473         /* return best, which is just the one with lowest difference */
1474         if (d1 > d2) {
1475                 copy_v3_v3(eul, eul2);
1476         }
1477         else {
1478                 copy_v3_v3(eul, eul1);
1479         }
1480 }
1481
1482 void mat4_to_compatible_eulO(float eul[3], float oldrot[3], const short order, float M[4][4])
1483 {
1484         float m[3][3];
1485
1486         /* for now, we'll just do this the slow way (i.e. copying matrices) */
1487         copy_m3_m4(m, M);
1488         normalize_m3(m);
1489         mat3_to_compatible_eulO(eul, oldrot, order, m);
1490 }
1491 /* rotate the given euler by the given angle on the specified axis */
1492 /* NOTE: is this safe to do with different axis orders? */
1493
1494 void rotate_eulO(float beul[3], const short order, char axis, float ang)
1495 {
1496         float eul[3], mat1[3][3], mat2[3][3], totmat[3][3];
1497
1498         assert(axis >= 'X' && axis <= 'Z');
1499
1500         zero_v3(eul);
1501
1502         if (axis == 'X')
1503                 eul[0] = ang;
1504         else if (axis == 'Y')
1505                 eul[1] = ang;
1506         else
1507                 eul[2] = ang;
1508
1509         eulO_to_mat3(mat1, eul, order);
1510         eulO_to_mat3(mat2, beul, order);
1511
1512         mul_m3_m3m3(totmat, mat2, mat1);
1513
1514         mat3_to_eulO(beul, order, totmat);
1515 }
1516
1517 /* the matrix is written to as 3 axis vectors */
1518 void eulO_to_gimbal_axis(float gmat[3][3], const float eul[3], const short order)
1519 {
1520         const RotOrderInfo *R = GET_ROTATIONORDER_INFO(order);
1521
1522         float mat[3][3];
1523         float teul[3];
1524
1525         /* first axis is local */
1526         eulO_to_mat3(mat, eul, order);
1527         copy_v3_v3(gmat[R->axis[0]], mat[R->axis[0]]);
1528
1529         /* second axis is local minus first rotation */
1530         copy_v3_v3(teul, eul);
1531         teul[R->axis[0]] = 0;
1532         eulO_to_mat3(mat, teul, order);
1533         copy_v3_v3(gmat[R->axis[1]], mat[R->axis[1]]);
1534
1535
1536         /* Last axis is global */
1537         zero_v3(gmat[R->axis[2]]);
1538         gmat[R->axis[2]][R->axis[2]] = 1;
1539 }
1540
1541 /******************************* Dual Quaternions ****************************/
1542
1543 /**
1544  * Conversion routines between (regular quaternion, translation) and
1545  * dual quaternion.
1546  *
1547  * Version 1.0.0, February 7th, 2007
1548  *
1549  * Copyright (C) 2006-2007 University of Dublin, Trinity College, All Rights
1550  * Reserved
1551  *
1552  * This software is provided 'as-is', without any express or implied
1553  * warranty.  In no event will the author(s) be held liable for any damages
1554  * arising from the use of this software.
1555  *
1556  * Permission is granted to anyone to use this software for any purpose,
1557  * including commercial applications, and to alter it and redistribute it
1558  * freely, subject to the following restrictions:
1559  *
1560  * 1. The origin of this software must not be misrepresented; you must not
1561  *    claim that you wrote the original software. If you use this software
1562  *    in a product, an acknowledgment in the product documentation would be
1563  *    appreciated but is not required.
1564  * 2. Altered source versions must be plainly marked as such, and must not be
1565  *    misrepresented as being the original software.
1566  * 3. This notice may not be removed or altered from any source distribution.
1567  *
1568  * \author Ladislav Kavan, kavanl@cs.tcd.ie
1569  *
1570  * Changes for Blender:
1571  * - renaming, style changes and optimization's
1572  * - added support for scaling
1573  */
1574
1575 void mat4_to_dquat(DualQuat *dq, float basemat[4][4], float mat[4][4])
1576 {
1577         float *t, *q, dscale[3], scale[3], basequat[4];
1578         float baseRS[4][4], baseinv[4][4], baseR[4][4], baseRinv[4][4];
1579         float R[4][4], S[4][4];
1580
1581         /* split scaling and rotation, there is probably a faster way to do
1582          * this, it's done like this now to correctly get negative scaling */
1583         mul_m4_m4m4(baseRS, mat, basemat);
1584         mat4_to_size(scale, baseRS);
1585
1586         dscale[0] = scale[0] - 1.0f;
1587         dscale[1] = scale[1] - 1.0f;
1588         dscale[2] = scale[2] - 1.0f;
1589
1590         if ((determinant_m4(mat) < 0.0f) || len_v3(dscale) > 1e-4f) {
1591                 /* extract R and S  */
1592                 float tmp[4][4];
1593
1594                 /* extra orthogonalize, to avoid flipping with stretched bones */
1595                 copy_m4_m4(tmp, baseRS);
1596                 orthogonalize_m4(tmp, 1);
1597                 mat4_to_quat(basequat, tmp);
1598
1599                 quat_to_mat4(baseR, basequat);
1600                 copy_v3_v3(baseR[3], baseRS[3]);
1601
1602                 invert_m4_m4(baseinv, basemat);
1603                 mul_m4_m4m4(R, baseR, baseinv);
1604
1605                 invert_m4_m4(baseRinv, baseR);
1606                 mul_m4_m4m4(S, baseRinv, baseRS);
1607
1608                 /* set scaling part */
1609                 mul_m4_series(dq->scale, basemat, S, baseinv);
1610                 dq->scale_weight = 1.0f;
1611         }
1612         else {
1613                 /* matrix does not contain scaling */
1614                 copy_m4_m4(R, mat);
1615                 dq->scale_weight = 0.0f;
1616         }
1617
1618         /* non-dual part */
1619         mat4_to_quat(dq->quat, R);
1620
1621         /* dual part */
1622         t = R[3];
1623         q = dq->quat;
1624         dq->trans[0] = -0.5f * ( t[0] * q[1] + t[1] * q[2] + t[2] * q[3]);
1625         dq->trans[1] =  0.5f * ( t[0] * q[0] + t[1] * q[3] - t[2] * q[2]);
1626         dq->trans[2] =  0.5f * (-t[0] * q[3] + t[1] * q[0] + t[2] * q[1]);
1627         dq->trans[3] =  0.5f * ( t[0] * q[2] - t[1] * q[1] + t[2] * q[0]);
1628 }
1629
1630 void dquat_to_mat4(float mat[4][4], const DualQuat *dq)
1631 {
1632         float len, q0[4];
1633         const float *t;
1634
1635         /* regular quaternion */
1636         copy_qt_qt(q0, dq->quat);
1637
1638         /* normalize */
1639         len = sqrtf(dot_qtqt(q0, q0));
1640         if (len != 0.0f)
1641                 mul_qt_fl(q0, 1.0f / len);
1642
1643         /* rotation */
1644         quat_to_mat4(mat, q0);
1645
1646         /* translation */
1647         t = dq->trans;
1648         mat[3][0] = 2.0f * (-t[0] * q0[1] + t[1] * q0[0] - t[2] * q0[3] + t[3] * q0[2]);
1649         mat[3][1] = 2.0f * (-t[0] * q0[2] + t[1] * q0[3] + t[2] * q0[0] - t[3] * q0[1]);
1650         mat[3][2] = 2.0f * (-t[0] * q0[3] - t[1] * q0[2] + t[2] * q0[1] + t[3] * q0[0]);
1651
1652         /* note: this does not handle scaling */
1653 }
1654
1655 void add_weighted_dq_dq(DualQuat *dqsum, const DualQuat *dq, float weight)
1656 {
1657         bool flipped = false;
1658
1659         /* make sure we interpolate quats in the right direction */
1660         if (dot_qtqt(dq->quat, dqsum->quat) < 0) {
1661                 flipped = 1;
1662                 weight = -weight;
1663         }
1664
1665         /* interpolate rotation and translation */
1666         dqsum->quat[0] += weight * dq->quat[0];
1667         dqsum->quat[1] += weight * dq->quat[1];
1668         dqsum->quat[2] += weight * dq->quat[2];
1669         dqsum->quat[3] += weight * dq->quat[3];
1670
1671         dqsum->trans[0] += weight * dq->trans[0];
1672         dqsum->trans[1] += weight * dq->trans[1];
1673         dqsum->trans[2] += weight * dq->trans[2];
1674         dqsum->trans[3] += weight * dq->trans[3];
1675
1676         /* interpolate scale - but only if needed */
1677         if (dq->scale_weight) {
1678                 float wmat[4][4];
1679
1680                 if (flipped) /* we don't want negative weights for scaling */
1681                         weight = -weight;
1682
1683                 copy_m4_m4(wmat, (float(*)[4])dq->scale);
1684                 mul_m4_fl(wmat, weight);
1685                 add_m4_m4m4(dqsum->scale, dqsum->scale, wmat);
1686                 dqsum->scale_weight += weight;
1687         }
1688 }
1689
1690 void normalize_dq(DualQuat *dq, float totweight)
1691 {
1692         float scale = 1.0f / totweight;
1693
1694         mul_qt_fl(dq->quat, scale);
1695         mul_qt_fl(dq->trans, scale);
1696
1697         if (dq->scale_weight) {
1698                 float addweight = totweight - dq->scale_weight;
1699
1700                 if (addweight) {
1701                         dq->scale[0][0] += addweight;
1702                         dq->scale[1][1] += addweight;
1703                         dq->scale[2][2] += addweight;
1704                         dq->scale[3][3] += addweight;
1705                 }
1706
1707                 mul_m4_fl(dq->scale, scale);
1708                 dq->scale_weight = 1.0f;
1709         }
1710 }
1711
1712 void mul_v3m3_dq(float co[3], float mat[3][3], DualQuat *dq)
1713 {
1714         float M[3][3], t[3], scalemat[3][3], len2;
1715         float w = dq->quat[0], x = dq->quat[1], y = dq->quat[2], z = dq->quat[3];
1716         float t0 = dq->trans[0], t1 = dq->trans[1], t2 = dq->trans[2], t3 = dq->trans[3];
1717
1718         /* rotation matrix */
1719         M[0][0] = w * w + x * x - y * y - z * z;
1720         M[1][0] = 2 * (x * y - w * z);
1721         M[2][0] = 2 * (x * z + w * y);
1722
1723         M[0][1] = 2 * (x * y + w * z);
1724         M[1][1] = w * w + y * y - x * x - z * z;
1725         M[2][1] = 2 * (y * z - w * x);
1726
1727         M[0][2] = 2 * (x * z - w * y);
1728         M[1][2] = 2 * (y * z + w * x);
1729         M[2][2] = w * w + z * z - x * x - y * y;
1730
1731         len2 = dot_qtqt(dq->quat, dq->quat);
1732         if (len2 > 0.0f)
1733                 len2 = 1.0f / len2;
1734
1735         /* translation */
1736         t[0] = 2 * (-t0 * x + w * t1 - t2 * z + y * t3);
1737         t[1] = 2 * (-t0 * y + t1 * z - x * t3 + w * t2);
1738         t[2] = 2 * (-t0 * z + x * t2 + w * t3 - t1 * y);
1739
1740         /* apply scaling */
1741         if (dq->scale_weight)
1742                 mul_m4_v3(dq->scale, co);
1743
1744         /* apply rotation and translation */
1745         mul_m3_v3(M, co);
1746         co[0] = (co[0] + t[0]) * len2;
1747         co[1] = (co[1] + t[1]) * len2;
1748         co[2] = (co[2] + t[2]) * len2;
1749
1750         /* compute crazyspace correction mat */
1751         if (mat) {
1752                 if (dq->scale_weight) {
1753                         copy_m3_m4(scalemat, dq->scale);
1754                         mul_m3_m3m3(mat, M, scalemat);
1755                 }
1756                 else
1757                         copy_m3_m3(mat, M);
1758                 mul_m3_fl(mat, len2);
1759         }
1760 }
1761
1762 void copy_dq_dq(DualQuat *dq1, const DualQuat *dq2)
1763 {
1764         memcpy(dq1, dq2, sizeof(DualQuat));
1765 }
1766
1767 /* axis matches eTrackToAxis_Modes */
1768 void quat_apply_track(float quat[4], short axis, short upflag)
1769 {
1770         /* rotations are hard coded to match vec_to_quat */
1771         const float sqrt_1_2 = (float)M_SQRT1_2;
1772         const float quat_track[][4] = {
1773                 {sqrt_1_2, 0.0, -sqrt_1_2, 0.0}, /* pos-y90 */
1774                 {0.5, 0.5, 0.5, 0.5}, /* Quaternion((1,0,0), radians(90)) * Quaternion((0,1,0), radians(90)) */
1775                 {sqrt_1_2, 0.0, 0.0, sqrt_1_2}, /* pos-z90 */
1776                 {sqrt_1_2, 0.0, sqrt_1_2, 0.0}, /* neg-y90 */
1777                 {0.5, -0.5, -0.5, 0.5}, /* Quaternion((1,0,0), radians(-90)) * Quaternion((0,1,0), radians(-90)) */
1778                 {0.0, sqrt_1_2, sqrt_1_2, 0.0} /* no rotation */
1779         };
1780
1781         assert(axis >= 0 && axis <= 5);
1782         assert(upflag >= 0 && upflag <= 2);
1783
1784         mul_qt_qtqt(quat, quat, quat_track[axis]);
1785
1786         if (axis > 2) {
1787                 axis = (short)(axis - 3);
1788         }
1789
1790         /* there are 2 possible up-axis for each axis used, the 'quat_track' applies so the first
1791          * up axis is used X->Y, Y->X, Z->X, if this first up axis isn't used then rotate 90d
1792          * the strange bit shift below just find the low axis {X:Y, Y:X, Z:X} */
1793         if (upflag != (2 - axis) >> 1) {
1794                 float q[4] = {sqrt_1_2, 0.0, 0.0, 0.0}; /* assign 90d rotation axis */
1795                 q[axis + 1] = ((axis == 1)) ? sqrt_1_2 : -sqrt_1_2; /* flip non Y axis */
1796                 mul_qt_qtqt(quat, quat, q);
1797         }
1798 }
1799
1800 void vec_apply_track(float vec[3], short axis)
1801 {
1802         float tvec[3];
1803
1804         assert(axis >= 0 && axis <= 5);
1805
1806         copy_v3_v3(tvec, vec);
1807
1808         switch (axis) {
1809                 case 0: /* pos-x */
1810                         /* vec[0] =  0.0; */
1811                         vec[1] = tvec[2];
1812                         vec[2] = -tvec[1];
1813                         break;
1814                 case 1: /* pos-y */
1815                         /* vec[0] = tvec[0]; */
1816                         /* vec[1] =  0.0; */
1817                         /* vec[2] = tvec[2]; */
1818                         break;
1819                 case 2: /* pos-z */
1820                         /* vec[0] = tvec[0]; */
1821                         /* vec[1] = tvec[1]; */
1822                         /* vec[2] =  0.0; */
1823                         break;
1824                 case 3: /* neg-x */
1825                         /* vec[0] =  0.0; */
1826                         vec[1] = tvec[2];
1827                         vec[2] = -tvec[1];
1828                         break;
1829                 case 4: /* neg-y */
1830                         vec[0] = -tvec[2];
1831                         /* vec[1] =  0.0; */
1832                         vec[2] = tvec[0];
1833                         break;
1834                 case 5: /* neg-z */
1835                         vec[0] = -tvec[0];
1836                         vec[1] = -tvec[1];
1837                         /* vec[2] =  0.0; */
1838                         break;
1839         }
1840 }
1841
1842 /* lens/angle conversion (radians) */
1843 float focallength_to_fov(float focal_length, float sensor)
1844 {
1845         return 2.0f * atanf((sensor / 2.0f) / focal_length);
1846 }
1847
1848 float fov_to_focallength(float hfov, float sensor)
1849 {
1850         return (sensor / 2.0f) / tanf(hfov * 0.5f);
1851 }
1852
1853 /* 'mod_inline(-3, 4)= 1', 'fmod(-3, 4)= -3' */
1854 static float mod_inline(float a, float b)
1855 {
1856         return a - (b * floorf(a / b));
1857 }
1858
1859 float angle_wrap_rad(float angle)
1860 {
1861         return mod_inline(angle + (float)M_PI, (float)M_PI * 2.0f) - (float)M_PI;
1862 }
1863
1864 float angle_wrap_deg(float angle)
1865 {
1866         return mod_inline(angle + 180.0f, 360.0f) - 180.0f;
1867 }
1868
1869 /* returns an angle compatible with angle_compat */
1870 float angle_compat_rad(float angle, float angle_compat)
1871 {
1872         return angle + (floorf(((angle_compat - angle) / (float)M_PI) + 0.5f)) * (float)M_PI;
1873 }
1874
1875 /* axis conversion */
1876 static float _axis_convert_matrix[23][3][3] = {
1877         {{-1.0, 0.0, 0.0}, {0.0, -1.0, 0.0}, {0.0, 0.0, 1.0}},
1878         {{-1.0, 0.0, 0.0}, {0.0, 0.0, -1.0}, {0.0, -1.0, 0.0}},
1879         {{-1.0, 0.0, 0.0}, {0.0, 0.0, 1.0}, {0.0, 1.0, 0.0}},
1880         {{-1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, -1.0}},
1881         {{0.0, -1.0, 0.0}, {-1.0, 0.0, 0.0}, {0.0, 0.0, -1.0}},
1882         {{0.0, 0.0, 1.0}, {-1.0, 0.0, 0.0}, {0.0, -1.0, 0.0}},
1883         {{0.0, 0.0, -1.0}, {-1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}},
1884         {{0.0, 1.0, 0.0}, {-1.0, 0.0, 0.0}, {0.0, 0.0, 1.0}},
1885         {{0.0, -1.0, 0.0}, {0.0, 0.0, 1.0}, {-1.0, 0.0, 0.0}},
1886         {{0.0, 0.0, -1.0}, {0.0, -1.0, 0.0}, {-1.0, 0.0, 0.0}},
1887         {{0.0, 0.0, 1.0}, {0.0, 1.0, 0.0}, {-1.0, 0.0, 0.0}},
1888         {{0.0, 1.0, 0.0}, {0.0, 0.0, -1.0}, {-1.0, 0.0, 0.0}},
1889         {{0.0, -1.0, 0.0}, {0.0, 0.0, -1.0}, {1.0, 0.0, 0.0}},
1890         {{0.0, 0.0, 1.0}, {0.0, -1.0, 0.0}, {1.0, 0.0, 0.0}},
1891         {{0.0, 0.0, -1.0}, {0.0, 1.0, 0.0}, {1.0, 0.0, 0.0}},
1892         {{0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}, {1.0, 0.0, 0.0}},
1893         {{0.0, -1.0, 0.0}, {1.0, 0.0, 0.0}, {0.0, 0.0, 1.0}},
1894         {{0.0, 0.0, -1.0}, {1.0, 0.0, 0.0}, {0.0, -1.0, 0.0}},
1895         {{0.0, 0.0, 1.0}, {1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}},
1896         {{0.0, 1.0, 0.0}, {1.0, 0.0, 0.0}, {0.0, 0.0, -1.0}},
1897         {{1.0, 0.0, 0.0}, {0.0, -1.0, 0.0}, {0.0, 0.0, -1.0}},
1898         {{1.0, 0.0, 0.0}, {0.0, 0.0, 1.0}, {0.0, -1.0, 0.0}},
1899         {{1.0, 0.0, 0.0}, {0.0, 0.0, -1.0}, {0.0, 1.0, 0.0}},
1900 };
1901
1902 static int _axis_convert_lut[23][24] = {
1903         {0x8C8, 0x4D0, 0x2E0, 0xAE8, 0x701, 0x511, 0x119, 0xB29, 0x682, 0x88A,
1904          0x09A, 0x2A2, 0x80B, 0x413, 0x223, 0xA2B, 0x644, 0x454, 0x05C, 0xA6C,
1905          0x745, 0x94D, 0x15D, 0x365},
1906         {0xAC8, 0x8D0, 0x4E0, 0x2E8, 0x741, 0x951, 0x159, 0x369, 0x702, 0xB0A,
1907          0x11A, 0x522, 0xA0B, 0x813, 0x423, 0x22B, 0x684, 0x894, 0x09C, 0x2AC,
1908          0x645, 0xA4D, 0x05D, 0x465},
1909         {0x4C8, 0x2D0, 0xAE0, 0x8E8, 0x681, 0x291, 0x099, 0x8A9, 0x642, 0x44A,
1910          0x05A, 0xA62, 0x40B, 0x213, 0xA23, 0x82B, 0x744, 0x354, 0x15C, 0x96C,
1911          0x705, 0x50D, 0x11D, 0xB25},
1912         {0x2C8, 0xAD0, 0x8E0, 0x4E8, 0x641, 0xA51, 0x059, 0x469, 0x742, 0x34A,
1913          0x15A, 0x962, 0x20B, 0xA13, 0x823, 0x42B, 0x704, 0xB14, 0x11C, 0x52C,
1914          0x685, 0x28D, 0x09D, 0x8A5},
1915         {0x708, 0xB10, 0x120, 0x528, 0x8C1, 0xAD1, 0x2D9, 0x4E9, 0x942, 0x74A,
1916          0x35A, 0x162, 0x64B, 0xA53, 0x063, 0x46B, 0x804, 0xA14, 0x21C, 0x42C,
1917          0x885, 0x68D, 0x29D, 0x0A5},
1918         {0xB08, 0x110, 0x520, 0x728, 0x941, 0x151, 0x359, 0x769, 0x802, 0xA0A,
1919          0x21A, 0x422, 0xA4B, 0x053, 0x463, 0x66B, 0x884, 0x094, 0x29C, 0x6AC,
1920          0x8C5, 0xACD, 0x2DD, 0x4E5},
1921         {0x508, 0x710, 0xB20, 0x128, 0x881, 0x691, 0x299, 0x0A9, 0x8C2, 0x4CA,
1922          0x2DA, 0xAE2, 0x44B, 0x653, 0xA63, 0x06B, 0x944, 0x754, 0x35C, 0x16C,
1923          0x805, 0x40D, 0x21D, 0xA25},
1924         {0x108, 0x510, 0x720, 0xB28, 0x801, 0x411, 0x219, 0xA29, 0x882, 0x08A,
1925          0x29A, 0x6A2, 0x04B, 0x453, 0x663, 0xA6B, 0x8C4, 0x4D4, 0x2DC, 0xAEC,
1926          0x945, 0x14D, 0x35D, 0x765},
1927         {0x748, 0x350, 0x160, 0x968, 0xAC1, 0x2D1, 0x4D9, 0x8E9, 0xA42, 0x64A,
1928          0x45A, 0x062, 0x68B, 0x293, 0x0A3, 0x8AB, 0xA04, 0x214, 0x41C, 0x82C,
1929          0xB05, 0x70D, 0x51D, 0x125},
1930         {0x948, 0x750, 0x360, 0x168, 0xB01, 0x711, 0x519, 0x129, 0xAC2, 0x8CA,
1931          0x4DA, 0x2E2, 0x88B, 0x693, 0x2A3, 0x0AB, 0xA44, 0x654, 0x45C, 0x06C,
1932          0xA05, 0x80D, 0x41D, 0x225},
1933         {0x348, 0x150, 0x960, 0x768, 0xA41, 0x051, 0x459, 0x669, 0xA02, 0x20A,
1934          0x41A, 0x822, 0x28B, 0x093, 0x8A3, 0x6AB, 0xB04, 0x114, 0x51C, 0x72C,
1935          0xAC5, 0x2CD, 0x4DD, 0x8E5},
1936         {0x148, 0x950, 0x760, 0x368, 0xA01, 0x811, 0x419, 0x229, 0xB02, 0x10A,
1937          0x51A, 0x722, 0x08B, 0x893, 0x6A3, 0x2AB, 0xAC4, 0x8D4, 0x4DC, 0x2EC,
1938          0xA45, 0x04D, 0x45D, 0x665},
1939         {0x688, 0x890, 0x0A0, 0x2A8, 0x4C1, 0x8D1, 0xAD9, 0x2E9, 0x502, 0x70A,
1940          0xB1A, 0x122, 0x74B, 0x953, 0x163, 0x36B, 0x404, 0x814, 0xA1C, 0x22C,
1941          0x445, 0x64D, 0xA5D, 0x065},
1942         {0x888, 0x090, 0x2A0, 0x6A8, 0x501, 0x111, 0xB19, 0x729, 0x402, 0x80A,
1943          0xA1A, 0x222, 0x94B, 0x153, 0x363, 0x76B, 0x444, 0x054, 0xA5C, 0x66C,
1944          0x4C5, 0x8CD, 0xADD, 0x2E5},
1945         {0x288, 0x690, 0x8A0, 0x0A8, 0x441, 0x651, 0xA59, 0x069, 0x4C2, 0x2CA,
1946          0xADA, 0x8E2, 0x34B, 0x753, 0x963, 0x16B, 0x504, 0x714, 0xB1C, 0x12C,
1947          0x405, 0x20D, 0xA1D, 0x825},
1948         {0x088, 0x290, 0x6A0, 0x8A8, 0x401, 0x211, 0xA19, 0x829, 0x442, 0x04A,
1949          0xA5A, 0x662, 0x14B, 0x353, 0x763, 0x96B, 0x4C4, 0x2D4, 0xADC, 0x8EC,
1950          0x505, 0x10D, 0xB1D, 0x725},
1951         {0x648, 0x450, 0x060, 0xA68, 0x2C1, 0x4D1, 0x8D9, 0xAE9, 0x282, 0x68A,
1952          0x89A, 0x0A2, 0x70B, 0x513, 0x123, 0xB2B, 0x204, 0x414, 0x81C, 0xA2C,
1953          0x345, 0x74D, 0x95D, 0x165},
1954         {0xA48, 0x650, 0x460, 0x068, 0x341, 0x751, 0x959, 0x169, 0x2C2, 0xACA,
1955          0x8DA, 0x4E2, 0xB0B, 0x713, 0x523, 0x12B, 0x284, 0x694, 0x89C, 0x0AC,
1956          0x205, 0xA0D, 0x81D, 0x425},
1957         {0x448, 0x050, 0xA60, 0x668, 0x281, 0x091, 0x899, 0x6A9, 0x202, 0x40A,
1958          0x81A, 0xA22, 0x50B, 0x113, 0xB23, 0x72B, 0x344, 0x154, 0x95C, 0x76C,
1959          0x2C5, 0x4CD, 0x8DD, 0xAE5},
1960         {0x048, 0xA50, 0x660, 0x468, 0x201, 0xA11, 0x819, 0x429, 0x342, 0x14A,
1961          0x95A, 0x762, 0x10B, 0xB13, 0x723, 0x52B, 0x2C4, 0xAD4, 0x8DC, 0x4EC,
1962          0x285, 0x08D, 0x89D, 0x6A5},
1963         {0x808, 0xA10, 0x220, 0x428, 0x101, 0xB11, 0x719, 0x529, 0x142, 0x94A,
1964          0x75A, 0x362, 0x8CB, 0xAD3, 0x2E3, 0x4EB, 0x044, 0xA54, 0x65C, 0x46C,
1965          0x085, 0x88D, 0x69D, 0x2A5},
1966         {0xA08, 0x210, 0x420, 0x828, 0x141, 0x351, 0x759, 0x969, 0x042, 0xA4A,
1967          0x65A, 0x462, 0xACB, 0x2D3, 0x4E3, 0x8EB, 0x084, 0x294, 0x69C, 0x8AC,
1968          0x105, 0xB0D, 0x71D, 0x525},
1969         {0x408, 0x810, 0xA20, 0x228, 0x081, 0x891, 0x699, 0x2A9, 0x102, 0x50A,
1970          0x71A, 0xB22, 0x4CB, 0x8D3, 0xAE3, 0x2EB, 0x144, 0x954, 0x75C, 0x36C,
1971          0x045, 0x44D, 0x65D, 0xA65},
1972 };
1973
1974 // _axis_convert_num = {'X': 0, 'Y': 1, 'Z': 2, '-X': 3, '-Y': 4, '-Z': 5}
1975
1976 BLI_INLINE int _axis_signed(const int axis)
1977 {
1978         return (axis < 3) ? axis : axis - 3;
1979 }
1980
1981 /*
1982  * Each argument us an axis in ['X', 'Y', 'Z', '-X', '-Y', '-Z']
1983  * where the first 2 are a source and the second 2 are the target.
1984  */
1985 int mat3_from_axis_conversion(int from_forward, int from_up, int to_forward, int to_up,
1986                               float r_mat[3][3])
1987 {
1988         // from functools import reduce
1989         int value;
1990         unsigned int i;
1991
1992         if (from_forward == to_forward && from_up == to_up) {
1993                 unit_m3(r_mat);
1994                 return false;
1995         }
1996
1997         if ((_axis_signed(from_forward) == _axis_signed(from_up)) ||
1998             (_axis_signed(to_forward)   == _axis_signed(to_up)))
1999         {
2000                 /* we could assert here! */
2001                 unit_m3(r_mat);
2002                 return false;
2003         }
2004
2005         value = ((from_forward << (0 * 3)) |
2006                  (from_up      << (1 * 3)) |
2007                  (to_forward   << (2 * 3)) |
2008                  (to_up        << (3 * 3)));
2009
2010         for (i = 0; i < (sizeof(_axis_convert_matrix) / sizeof(*_axis_convert_matrix)); i++) {
2011                 unsigned int j;
2012                 for (j = 0; j < (sizeof(*_axis_convert_lut) / sizeof(*_axis_convert_lut[0])); j++) {
2013                         if (_axis_convert_lut[i][j] == value) {
2014                                 copy_m3_m3(r_mat, _axis_convert_matrix[i]);
2015                                 return true;
2016                         }
2017                 }
2018
2019         }
2020 //      BLI_assert(0);
2021         return false;
2022 }