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