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