Blender V5.3
BLI_math_matrix.hh
Go to the documentation of this file.
1/* SPDX-FileCopyrightText: 2022 Blender Authors
2 *
3 * SPDX-License-Identifier: GPL-2.0-or-later */
4
5#pragma once
6
10
11#include "BLI_math_base.hh"
14#include "BLI_math_vector.hh"
15#include "BLI_unroll.hh"
16
17namespace blender::math {
18
19/* -------------------------------------------------------------------- */
22
27template<typename T, int Size>
28[[nodiscard]] MatBase<T, Size, Size> invert(const MatBase<T, Size, Size> &mat, bool &r_success);
29
33template<typename T, int NumCol, int NumRow>
34[[nodiscard]] MatBase<T, NumCol, NumRow> transpose(const MatBase<T, NumRow, NumCol> &mat);
35
39template<typename T, int NumCol, int NumRow>
40[[nodiscard]] MatBase<T, NumCol, NumRow> normalize(const MatBase<T, NumCol, NumRow> &a);
41
46template<typename T, int NumCol, int NumRow, typename VectorT>
47[[nodiscard]] MatBase<T, NumCol, NumRow> normalize_and_get_size(
48 const MatBase<T, NumCol, NumRow> &a, VectorT &r_size);
49
54template<typename T, int Size> [[nodiscard]] T determinant(const MatBase<T, Size, Size> &mat);
55
59template<typename T, int Size>
60[[nodiscard]] MatBase<T, Size, Size> adjoint(const MatBase<T, Size, Size> &mat);
61
65template<typename T, int NumCol, int NumRow, typename VectorT>
66[[nodiscard]] MatBase<T, NumCol, NumRow> translate(const MatBase<T, NumCol, NumRow> &mat,
67 const VectorT &translation);
68
73template<typename T, int NumCol, int NumRow, typename RotationT>
75 const RotationT &rotation);
76
80template<typename T, int NumCol, int NumRow, typename VectorT>
82 const VectorT &scale);
83
87template<typename T, int NumCol, int NumRow>
90 T t);
91
107template<typename T>
108[[nodiscard]] MatBase<T, 3, 3> interpolate(const MatBase<T, 3, 3> &a,
109 const MatBase<T, 3, 3> &b,
110 T t);
111
120template<typename T>
121[[nodiscard]] MatBase<T, 4, 4> interpolate(const MatBase<T, 4, 4> &a,
122 const MatBase<T, 4, 4> &b,
123 T t);
124
136template<typename T>
138 const MatBase<T, 3, 3> &b,
139 T t);
140
153template<typename T>
155 const MatBase<T, 4, 4> &b,
156 T t);
157
168template<typename T, int Size>
170 T epsilon = 1e-8);
171
173
174/* -------------------------------------------------------------------- */
177
181template<typename MatT> [[nodiscard]] MatT from_location(const typename MatT::loc_type &location);
182
187template<typename MatT, int ScaleDim>
189
193template<typename MatT, typename RotationT>
194[[nodiscard]] MatT from_rotation(const RotationT &rotation);
195
199template<typename MatT, typename RotationT, typename VectorT>
200[[nodiscard]] MatT from_rot_scale(const RotationT &rotation, const VectorT &scale);
201
205template<typename MatT, typename RotationT>
206[[nodiscard]] MatT from_loc_rot(const typename MatT::loc_type &location,
207 const RotationT &rotation);
208
212template<typename MatT, int ScaleDim>
213[[nodiscard]] MatT from_loc_scale(const typename MatT::loc_type &location,
215
219template<typename MatT, typename RotationT, int ScaleDim>
220[[nodiscard]] MatT from_loc_rot_scale(const typename MatT::loc_type &location,
221 const RotationT &rotation,
223
228template<typename T> [[nodiscard]] MatBase<T, 2, 2> from_direction(const VecBase<T, 2> &direction);
229
235template<typename MatT, typename VectorT>
236[[nodiscard]] MatT from_orthonormal_axes(const VectorT forward, const VectorT up);
237
242template<typename MatT, typename VectorT>
243[[nodiscard]] MatT from_orthonormal_axes(const VectorT location,
244 const VectorT forward,
245 const VectorT up);
246
255template<typename MatT, typename VectorT> [[nodiscard]] MatT from_up_axis(const VectorT up);
256
266template<typename MatT> [[nodiscard]] MatT orthogonalize(const MatT &mat, const Axis axis);
267
273template<typename MatT, typename VectorT>
274[[nodiscard]] MatT from_origin_transform(const MatT &transform, const VectorT origin);
275
277
278/* -------------------------------------------------------------------- */
281
286template<typename T> [[nodiscard]] inline AngleRadianBase<T> to_angle(const MatBase<T, 2, 2> &mat);
287template<typename T> [[nodiscard]] inline EulerXYZBase<T> to_euler(const MatBase<T, 3, 3> &mat);
288template<typename T> [[nodiscard]] inline EulerXYZBase<T> to_euler(const MatBase<T, 4, 4> &mat);
289template<typename T>
290[[nodiscard]] inline Euler3Base<T> to_euler(const MatBase<T, 3, 3> &mat, EulerOrder order);
291template<typename T>
292[[nodiscard]] inline Euler3Base<T> to_euler(const MatBase<T, 4, 4> &mat, EulerOrder order);
293
301template<typename T>
302[[nodiscard]] inline EulerXYZBase<T> to_nearest_euler(const MatBase<T, 3, 3> &mat,
303 const EulerXYZBase<T> &reference);
304template<typename T>
305[[nodiscard]] inline EulerXYZBase<T> to_nearest_euler(const MatBase<T, 4, 4> &mat,
306 const EulerXYZBase<T> &reference);
307template<typename T>
308[[nodiscard]] inline Euler3Base<T> to_nearest_euler(const MatBase<T, 3, 3> &mat,
309 const Euler3Base<T> &reference);
310template<typename T>
311[[nodiscard]] inline Euler3Base<T> to_nearest_euler(const MatBase<T, 4, 4> &mat,
312 const Euler3Base<T> &reference);
313
317template<typename T>
318[[nodiscard]] inline QuaternionBase<T> to_quaternion(const MatBase<T, 3, 3> &mat);
319template<typename T>
320[[nodiscard]] inline QuaternionBase<T> to_quaternion(const MatBase<T, 4, 4> &mat);
321
328[[nodiscard]] Quaternion to_quaternion_legacy(const float3x3 &mat);
329
335template<bool AllowNegativeScale = false, typename T, int NumCol, int NumRow>
336[[nodiscard]] inline VecBase<T, 3> to_scale(const MatBase<T, NumCol, NumRow> &mat);
337template<bool AllowNegativeScale = false, typename T>
338[[nodiscard]] inline VecBase<T, 2> to_scale(const MatBase<T, 2, 2> &mat);
339
346template<bool AllowNegativeScale = false, typename T>
347inline void to_rot_scale(const MatBase<T, 2, 2> &mat,
348 AngleRadianBase<T> &r_rotation,
349 VecBase<T, 2> &r_scale);
350template<bool AllowNegativeScale = false, typename T>
351inline void to_loc_rot_scale(const MatBase<T, 3, 3> &mat,
352 VecBase<T, 2> &r_location,
353 AngleRadianBase<T> &r_rotation,
354 VecBase<T, 2> &r_scale);
355template<bool AllowNegativeScale = false, typename T, typename RotationT>
356inline void to_rot_scale(const MatBase<T, 3, 3> &mat,
357 RotationT &r_rotation,
358 VecBase<T, 3> &r_scale);
359template<bool AllowNegativeScale = false, typename T, typename RotationT>
360inline void to_loc_rot_scale(const MatBase<T, 4, 4> &mat,
361 VecBase<T, 3> &r_location,
362 RotationT &r_rotation,
363 VecBase<T, 3> &r_scale);
364
366
367/* -------------------------------------------------------------------- */
370
374template<typename T>
375[[nodiscard]] VecBase<T, 2> transform_point(const MatBase<T, 2, 2> &mat,
376 const VecBase<T, 2> &point);
377
381template<typename T>
382[[nodiscard]] VecBase<T, 2> transform_point(const MatBase<T, 3, 3> &mat,
383 const VecBase<T, 2> &point);
384
388template<typename T>
389[[nodiscard]] VecBase<T, 3> transform_point(const MatBase<T, 3, 3> &mat,
390 const VecBase<T, 3> &point);
391
395template<typename T>
396[[nodiscard]] VecBase<T, 3> transform_point(const MatBase<T, 4, 4> &mat,
397 const VecBase<T, 3> &point);
398
402template<typename T>
403[[nodiscard]] VecBase<T, 3> transform_direction(const MatBase<T, 3, 3> &mat,
404 const VecBase<T, 3> &direction);
405
409template<typename T>
410[[nodiscard]] VecBase<T, 3> transform_direction(const MatBase<T, 4, 4> &mat,
411 const VecBase<T, 3> &direction);
412
416template<typename MatT, typename VectorT>
417[[nodiscard]] VectorT project_point(const MatT &mat, const VectorT &point);
418
422template<typename MatT, typename VectorT>
423[[nodiscard]] VectorT project_point_safe(const MatT &mat, const VectorT &point);
424
426
427/* -------------------------------------------------------------------- */
430
431namespace projection {
432
438template<typename T>
439[[nodiscard]] MatBase<T, 4, 4> orthographic(
440 T left, T right, T bottom, T top, T near_clip, T far_clip);
441
449template<typename T> MatBase<T, 4, 4> orthographic_infinite(T left, T right, T bottom, T top);
450
457template<typename T>
458[[nodiscard]] MatBase<T, 4, 4> perspective(
459 T left, T right, T bottom, T top, T near_clip, T far_clip);
460
467template<typename T>
469 T angle_left, T angle_right, T angle_bottom, T angle_top, T near_clip, T far_clip);
470
478template<typename T>
479[[nodiscard]] MatBase<T, 4, 4> perspective_infinite(T left, T right, T bottom, T top, T near_clip);
480
486template<typename T>
487[[nodiscard]] MatBase<T, 4, 4> translate(const MatBase<T, 4, 4> &mat, const VecBase<T, 2> &offset);
488
489} // namespace projection
490
492
493/* -------------------------------------------------------------------- */
496
503template<typename T> [[nodiscard]] bool is_negative(const MatBase<T, 3, 3> &mat);
504template<typename T> [[nodiscard]] bool is_negative(const MatBase<T, 4, 4> &mat);
505
509template<typename T, int NumCol, int NumRow>
510[[nodiscard]] inline bool is_equal(const MatBase<T, NumCol, NumRow> &a,
512 const T epsilon = T(0))
513{
514 for (int i = 0; i < NumCol; i++) {
515 for (int j = 0; j < NumRow; j++) {
516 if (math::abs(a[i][j] - b[i][j]) > epsilon) {
517 return false;
518 }
519 }
520 }
521 return true;
522}
523
527template<typename T, int NumCol, int NumRow>
528[[nodiscard]] inline bool is_identity(const MatBase<T, NumCol, NumRow> &mat)
529{
530 for (int i = 0; i < NumCol; i++) {
531 for (int j = 0; j < NumRow; j++) {
532 if (mat[i][j] != (i != j ? 0.0f : 1.0f)) {
533 return false;
534 }
535 }
536 }
537 return true;
538}
539
543template<typename MatT> [[nodiscard]] inline bool is_orthogonal(const MatT &mat)
544{
545 if (math::abs(math::dot(mat.x_axis(), mat.y_axis())) > 1e-5f) {
546 return false;
547 }
548 if (math::abs(math::dot(mat.y_axis(), mat.z_axis())) > 1e-5f) {
549 return false;
550 }
551 if (math::abs(math::dot(mat.z_axis(), mat.x_axis())) > 1e-5f) {
552 return false;
553 }
554 return true;
555}
556
560template<typename MatT> [[nodiscard]] inline bool is_orthonormal(const MatT &mat)
561{
562 if (!is_orthogonal(mat)) {
563 return false;
564 }
565 if (math::abs(math::length_squared(mat.x_axis()) - 1) > 1e-5f) {
566 return false;
567 }
568 if (math::abs(math::length_squared(mat.y_axis()) - 1) > 1e-5f) {
569 return false;
570 }
571 if (math::abs(math::length_squared(mat.z_axis()) - 1) > 1e-5f) {
572 return false;
573 }
574 return true;
575}
576
580template<typename MatT> [[nodiscard]] inline bool is_uniformly_scaled(const MatT &mat)
581{
582 if (!is_orthogonal(mat)) {
583 return false;
584 }
585 using T = typename MatT::base_type;
586 const T eps = 1e-7;
587 const T x = math::length_squared(mat.x_axis());
588 const T y = math::length_squared(mat.y_axis());
589 const T z = math::length_squared(mat.z_axis());
590 return (math::abs(x - y) < eps) && math::abs(x - z) < eps;
591}
592
593template<typename T, int NumCol, int NumRow>
594inline bool is_zero(const MatBase<T, NumCol, NumRow> &mat)
595{
596 for (int i = 0; i < NumCol; i++) {
597 if (!is_zero(mat[i])) {
598 return false;
599 }
600 }
601 return true;
602}
603
605
606/* -------------------------------------------------------------------- */
609
610/* Implementation details. */
611namespace detail {
612
613template<typename T, int NumCol, int NumRow>
614[[nodiscard]] MatBase<T, NumCol, NumRow> from_rotation(const AngleRadianBase<T> &rotation);
615
616template<typename T, int NumCol, int NumRow>
617[[nodiscard]] MatBase<T, NumCol, NumRow> from_rotation(const EulerXYZBase<T> &rotation);
618
619template<typename T, int NumCol, int NumRow>
620[[nodiscard]] MatBase<T, NumCol, NumRow> from_rotation(const Euler3Base<T> &rotation);
621
622template<typename T, int NumCol, int NumRow>
623[[nodiscard]] MatBase<T, NumCol, NumRow> from_rotation(const QuaternionBase<T> &rotation);
624
625template<typename T, int NumCol, int NumRow>
627
628template<typename T, int NumCol, int NumRow>
629[[nodiscard]] MatBase<T, NumCol, NumRow> from_rotation(const CartesianBasis &rotation);
630
631template<typename T, int NumCol, int NumRow, typename AngleT>
633
634} // namespace detail
635
636/* Returns true if each individual columns are unit scaled. Mainly for assert usage. */
637template<typename T, int NumCol, int NumRow>
638[[nodiscard]] inline bool is_unit_scale(const MatBase<T, NumCol, NumRow> &m)
639{
640 for (int i = 0; i < NumCol; i++) {
641 if (!is_unit_scale(m[i])) {
642 return false;
643 }
644 }
645 return true;
646}
647
648template<typename T, int Size>
650{
651 bool success;
652 /* Explicit template parameter to please MSVC. */
653 return invert<T, Size>(mat, success);
654}
655
656template<typename T, int NumCol, int NumRow>
658{
660 unroll<NumCol>([&](auto i) { unroll<NumRow>([&](auto j) { result[i][j] = mat[j][i]; }); });
661 return result;
662}
663
664template<typename T, int NumCol, int NumRow, typename VectorT>
666 const VectorT &translation)
667{
668 using MatT = MatBase<T, NumCol, NumRow>;
669 BLI_STATIC_ASSERT(VectorT::type_length <= MatT::col_len - 1,
670 "Translation should be at least 1 column less than the matrix.");
671 constexpr int location_col = MatT::col_len - 1;
672 /* Avoid multiplying the last row if it exists.
673 * Allows using non square matrices like float3x2 and saves computation. */
674 using IntermediateVecT =
675 VecBase<typename MatT::base_type,
676 (MatT::row_len > MatT::col_len - 1) ? (MatT::col_len - 1) : MatT::row_len>;
677
678 MatT result = mat;
679 unroll<VectorT::type_length>([&](auto c) {
680 *reinterpret_cast<IntermediateVecT *>(
681 &result[location_col]) += translation[c] *
682 *reinterpret_cast<const IntermediateVecT *>(&mat[c]);
683 });
684 return result;
685}
686
687template<typename T, int NumCol, int NumRow, typename AngleT>
689 const AxisAngleBase<T, AngleT> &rotation)
690{
691 using MatT = MatBase<T, NumCol, NumRow>;
692 using Vec3T = typename MatT::vec3_type;
693 const T angle_sin = sin(rotation.angle());
694 const T angle_cos = cos(rotation.angle());
695 const Vec3T &axis_vec = rotation.axis();
696
697 MatT result = mat;
698 /* axis_vec is given to be normalized. */
699 if (axis_vec.x == T(1)) {
700 unroll<MatT::row_len>([&](auto c) {
701 result[2][c] = -angle_sin * mat[1][c] + angle_cos * mat[2][c];
702 result[1][c] = angle_cos * mat[1][c] + angle_sin * mat[2][c];
703 });
704 }
705 else if (axis_vec.y == T(1)) {
706 unroll<MatT::row_len>([&](auto c) {
707 result[0][c] = angle_cos * mat[0][c] - angle_sin * mat[2][c];
708 result[2][c] = angle_sin * mat[0][c] + angle_cos * mat[2][c];
709 });
710 }
711 else if (axis_vec.z == T(1)) {
712 unroll<MatT::row_len>([&](auto c) {
713 result[0][c] = angle_cos * mat[0][c] + angle_sin * mat[1][c];
714 result[1][c] = -angle_sin * mat[0][c] + angle_cos * mat[1][c];
715 });
716 }
717 else {
718 /* Un-optimized case. Arbitrary */
719 result *= from_rotation<MatT>(rotation);
720 }
721 return result;
722}
723
724template<typename T, int NumCol, int NumRow, typename VectorT>
726 const VectorT &scale)
727{
728 BLI_STATIC_ASSERT(VectorT::type_length <= NumCol,
729 "Scale should be less or equal to the matrix in column count.");
731 unroll<VectorT::type_length>([&](auto c) { result[c] *= scale[c]; });
732 return result;
733}
734
735template<typename T, int NumCol, int NumRow>
738 T t)
739{
741 unroll<NumCol>([&](auto c) { result[c] = interpolate(a[c], b[c], t); });
742 return result;
743}
744
745template<typename T,
746 int NumCol,
747 int NumRow,
748 int SrcNumCol,
749 int SrcNumRow,
750 int SrcStartCol,
751 int SrcStartRow,
752 int SrcAlignment>
761
762template<typename T,
763 int NumCol,
764 int NumRow,
765 int SrcNumCol,
766 int SrcNumRow,
767 int SrcStartCol,
768 int SrcStartRow,
769 int SrcAlignment,
770 typename VectorT>
773 &a,
774 VectorT &r_size)
775{
776 BLI_STATIC_ASSERT(VectorT::type_length == NumCol,
777 "r_size dimension should be equal to matrix column count.");
779 unroll<NumCol>([&](auto i) { result[i] = math::normalize_and_get_length(a[i], r_size[i]); });
780 return result;
781}
782
783template<typename T, int NumCol, int NumRow>
785{
787 unroll<NumCol>([&](auto i) { result[i] = math::normalize(a[i]); });
788 return result;
789}
790
791template<typename T, int NumCol, int NumRow, typename VectorT>
793 const MatBase<T, NumCol, NumRow> &a, VectorT &r_size)
794{
795 BLI_STATIC_ASSERT(VectorT::type_length == NumCol,
796 "r_size dimension should be equal to matrix column count.");
798 unroll<NumCol>([&](auto i) { result[i] = math::normalize_and_get_length(a[i], r_size[i]); });
799 return result;
800}
801
802namespace detail {
803
805{
807 return AngleRadianBase(mat[0][0], mat[0][1]);
808}
809
810template<typename T>
812{
814
815 const T cy = math::hypot(mat[0][0], mat[0][1]);
816 if (cy > T(16) * std::numeric_limits<T>::epsilon()) {
817 eul1.x() = math::atan2(mat[1][2], mat[2][2]);
818 eul1.y() = math::atan2(-mat[0][2], cy);
819 eul1.z() = math::atan2(mat[0][1], mat[0][0]);
820
821 eul2.x() = math::atan2(-mat[1][2], -mat[2][2]);
822 eul2.y() = math::atan2(-mat[0][2], -cy);
823 eul2.z() = math::atan2(-mat[0][1], -mat[0][0]);
824 }
825 else {
826 eul1.x() = math::atan2(-mat[2][1], mat[1][1]);
827 eul1.y() = math::atan2(-mat[0][2], cy);
828 eul1.z() = 0.0f;
829
830 eul2 = eul1;
831 }
832}
833template<typename T>
835{
837 const int i_index = eul1.i_index();
838 const int j_index = eul1.j_index();
839 const int k_index = eul1.k_index();
840
841 const T cy = math::hypot(mat[i_index][i_index], mat[i_index][j_index]);
842 if (cy > T(16) * std::numeric_limits<T>::epsilon()) {
843 eul1.i() = math::atan2(mat[j_index][k_index], mat[k_index][k_index]);
844 eul1.j() = math::atan2(-mat[i_index][k_index], cy);
845 eul1.k() = math::atan2(mat[i_index][j_index], mat[i_index][i_index]);
846
847 eul2.i() = math::atan2(-mat[j_index][k_index], -mat[k_index][k_index]);
848 eul2.j() = math::atan2(-mat[i_index][k_index], -cy);
849 eul2.k() = math::atan2(-mat[i_index][j_index], -mat[i_index][i_index]);
850 }
851 else {
852 eul1.i() = math::atan2(-mat[k_index][j_index], mat[j_index][j_index]);
853 eul1.j() = math::atan2(-mat[i_index][k_index], cy);
854 eul1.k() = 0.0f;
855
856 eul2 = eul1;
857 }
858
859 if (eul1.parity()) {
860 eul1 = -eul1;
861 eul2 = -eul2;
862 }
863}
864
865/* Using explicit template instantiations in order to reduce compilation time. */
866extern template void normalized_to_eul2(const float3x3 &mat,
867 Euler3Base<float> &eul1,
868 Euler3Base<float> &eul2);
869extern template void normalized_to_eul2(const float3x3 &mat,
871 EulerXYZBase<float> &eul2);
872extern template void normalized_to_eul2(const double3x3 &mat,
875
877{
879 /* Caller must ensure matrices aren't negative for valid results, see: #24291, #94231. */
881
883
884 /* Method outlined by Mike Day, ref: https://math.stackexchange.com/a/3183435/220949
885 * with an additional `sqrtf(..)` for higher precision result.
886 * Removing the `sqrt` causes tests to fail unless the precision is set to 1e-6 or larger. */
887
888 if (mat[2][2] < 0.0f) {
889 if (mat[0][0] > mat[1][1]) {
890 const T trace = 1.0f + mat[0][0] - mat[1][1] - mat[2][2];
891 T s = 2.0f * math::sqrt(trace);
892 if (mat[1][2] < mat[2][1]) {
893 /* Ensure W is non-negative for a canonical result. */
894 s = -s;
895 }
896 q.x = 0.25f * s;
897 s = 1.0f / s;
898 q.w = (mat[1][2] - mat[2][1]) * s;
899 q.y = (mat[0][1] + mat[1][0]) * s;
900 q.z = (mat[2][0] + mat[0][2]) * s;
901 if ((trace == 1.0f) && (q.w == 0.0f && q.y == 0.0f && q.z == 0.0f)) [[unlikely]] {
902 /* Avoids the need to normalize the degenerate case. */
903 q.x = 1.0f;
904 }
905 }
906 else {
907 const T trace = 1.0f - mat[0][0] + mat[1][1] - mat[2][2];
908 T s = 2.0f * math::sqrt(trace);
909 if (mat[2][0] < mat[0][2]) {
910 /* Ensure W is non-negative for a canonical result. */
911 s = -s;
912 }
913 q.y = 0.25f * s;
914 s = 1.0f / s;
915 q.w = (mat[2][0] - mat[0][2]) * s;
916 q.x = (mat[0][1] + mat[1][0]) * s;
917 q.z = (mat[1][2] + mat[2][1]) * s;
918 if ((trace == 1.0f) && (q.w == 0.0f && q.x == 0.0f && q.z == 0.0f)) [[unlikely]] {
919 /* Avoids the need to normalize the degenerate case. */
920 q.y = 1.0f;
921 }
922 }
923 }
924 else {
925 if (mat[0][0] < -mat[1][1]) {
926 const T trace = 1.0f - mat[0][0] - mat[1][1] + mat[2][2];
927 T s = 2.0f * math::sqrt(trace);
928 if (mat[0][1] < mat[1][0]) {
929 /* Ensure W is non-negative for a canonical result. */
930 s = -s;
931 }
932 q.z = 0.25f * s;
933 s = 1.0f / s;
934 q.w = (mat[0][1] - mat[1][0]) * s;
935 q.x = (mat[2][0] + mat[0][2]) * s;
936 q.y = (mat[1][2] + mat[2][1]) * s;
937 if ((trace == 1.0f) && (q.w == 0.0f && q.x == 0.0f && q.y == 0.0f)) [[unlikely]] {
938 /* Avoids the need to normalize the degenerate case. */
939 q.z = 1.0f;
940 }
941 }
942 else {
943 /* NOTE(@ideasman42): A zero matrix will fall through to this block,
944 * needed so a zero scaled matrices to return a quaternion without rotation, see: #101848.
945 */
946 const T trace = 1.0f + mat[0][0] + mat[1][1] + mat[2][2];
947 T s = 2.0f * math::sqrt(trace);
948 q.w = 0.25f * s;
949 s = 1.0f / s;
950 q.x = (mat[1][2] - mat[2][1]) * s;
951 q.y = (mat[2][0] - mat[0][2]) * s;
952 q.z = (mat[0][1] - mat[1][0]) * s;
953 if ((trace == 1.0f) && (q.x == 0.0f && q.y == 0.0f && q.z == 0.0f)) [[unlikely]] {
954 /* Avoids the need to normalize the degenerate case. */
955 q.w = 1.0f;
956 }
957 }
958 }
959
960 BLI_assert(!(q.w < 0.0f));
961
962 /* Sometimes normalization is necessary due to round-off errors in the above
963 * calculations. The comparison here uses tighter tolerances than
964 * BLI_ASSERT_UNIT_QUAT(), so it's likely that even after a few more
965 * transformations the quaternion will still be considered unit-ish. */
966 const T q_len_squared = q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
967 const T threshold = 0.0002f /* #BLI_ASSERT_UNIT_EPSILON */;
968 if (math::abs(q_len_squared - 1.0f) >= threshold) {
969 const T q_len_inv = 1.0 / math::sqrt(q_len_squared);
970 q.x *= q_len_inv;
971 q.y *= q_len_inv;
972 q.z *= q_len_inv;
973 q.w *= q_len_inv;
974 }
975
977 return q;
978}
979
981{
982 const T det = math::determinant(mat);
983 if (!std::isfinite(det)) [[unlikely]] {
985 }
986 if (det < T(0)) [[unlikely]] {
987 return normalized_to_quat_fast(-mat);
988 }
989 return normalized_to_quat_fast(mat);
990}
991
992/* Using explicit template instantiations in order to reduce compilation time. */
995
996template<typename T, int NumCol, int NumRow>
998{
999 using MatT = MatBase<T, NumCol, NumRow>;
1000 using DoublePrecision = typename TypeTraits<T>::DoublePrecision;
1001 const DoublePrecision cos_i = math::cos(DoublePrecision(rotation.x().radian()));
1002 const DoublePrecision cos_j = math::cos(DoublePrecision(rotation.y().radian()));
1003 const DoublePrecision cos_k = math::cos(DoublePrecision(rotation.z().radian()));
1004 const DoublePrecision sin_i = math::sin(DoublePrecision(rotation.x().radian()));
1005 const DoublePrecision sin_j = math::sin(DoublePrecision(rotation.y().radian()));
1006 const DoublePrecision sin_k = math::sin(DoublePrecision(rotation.z().radian()));
1007 const DoublePrecision cos_i_cos_k = cos_i * cos_k;
1008 const DoublePrecision cos_i_sin_k = cos_i * sin_k;
1009 const DoublePrecision sin_i_cos_k = sin_i * cos_k;
1010 const DoublePrecision sin_i_sin_k = sin_i * sin_k;
1011
1012 MatT mat = MatT::identity();
1013 mat[0][0] = T(cos_j * cos_k);
1014 mat[1][0] = T(sin_j * sin_i_cos_k - cos_i_sin_k);
1015 mat[2][0] = T(sin_j * cos_i_cos_k + sin_i_sin_k);
1016
1017 mat[0][1] = T(cos_j * sin_k);
1018 mat[1][1] = T(sin_j * sin_i_sin_k + cos_i_cos_k);
1019 mat[2][1] = T(sin_j * cos_i_sin_k - sin_i_cos_k);
1020
1021 mat[0][2] = T(-sin_j);
1022 mat[1][2] = T(cos_j * sin_i);
1023 mat[2][2] = T(cos_j * cos_i);
1024 return mat;
1025}
1026
1027template<typename T, int NumCol, int NumRow>
1029{
1030 using MatT = MatBase<T, NumCol, NumRow>;
1031 const int i_index = rotation.i_index();
1032 const int j_index = rotation.j_index();
1033 const int k_index = rotation.k_index();
1034#if 1 /* Reference. */
1035 EulerXYZBase<T> euler_xyz(rotation.ijk());
1036 const MatT mat = from_rotation<T, NumCol, NumRow>(rotation.parity() ? -euler_xyz : euler_xyz);
1037 MatT result = MatT::identity();
1038 result[i_index][i_index] = mat[0][0];
1039 result[j_index][i_index] = mat[1][0];
1040 result[k_index][i_index] = mat[2][0];
1041 result[i_index][j_index] = mat[0][1];
1042 result[j_index][j_index] = mat[1][1];
1043 result[k_index][j_index] = mat[2][1];
1044 result[i_index][k_index] = mat[0][2];
1045 result[j_index][k_index] = mat[1][2];
1046 result[k_index][k_index] = mat[2][2];
1047#else
1048 /* TODO(fclem): Manually inline and check performance difference. */
1049#endif
1050 return result;
1051}
1052
1053template<typename T, int NumCol, int NumRow>
1055{
1056 using MatT = MatBase<T, NumCol, NumRow>;
1057 using DoublePrecision = typename TypeTraits<T>::DoublePrecision;
1058 const DoublePrecision q0 = std::numbers::sqrt2 * DoublePrecision(rotation.w);
1059 const DoublePrecision q1 = std::numbers::sqrt2 * DoublePrecision(rotation.x);
1060 const DoublePrecision q2 = std::numbers::sqrt2 * DoublePrecision(rotation.y);
1061 const DoublePrecision q3 = std::numbers::sqrt2 * DoublePrecision(rotation.z);
1062
1063 const DoublePrecision qda = q0 * q1;
1064 const DoublePrecision qdb = q0 * q2;
1065 const DoublePrecision qdc = q0 * q3;
1066 const DoublePrecision qaa = q1 * q1;
1067 const DoublePrecision qab = q1 * q2;
1068 const DoublePrecision qac = q1 * q3;
1069 const DoublePrecision qbb = q2 * q2;
1070 const DoublePrecision qbc = q2 * q3;
1071 const DoublePrecision qcc = q3 * q3;
1072
1073 MatT mat = MatT::identity();
1074 mat[0][0] = T(1.0 - qbb - qcc);
1075 mat[0][1] = T(qdc + qab);
1076 mat[0][2] = T(-qdb + qac);
1077
1078 mat[1][0] = T(-qdc + qab);
1079 mat[1][1] = T(1.0 - qaa - qcc);
1080 mat[1][2] = T(qda + qbc);
1081
1082 mat[2][0] = T(qdb + qac);
1083 mat[2][1] = T(-qda + qbc);
1084 mat[2][2] = T(1.0 - qaa - qbb);
1085 return mat;
1086}
1087
1088/* Not technically speaking a simple rotation, but a whole transform. */
1089template<typename T, int NumCol, int NumRow>
1091{
1092 using MatT = MatBase<T, NumCol, NumRow>;
1093 BLI_assert(is_normalized(rotation));
1100 /* Follow the paper notation. */
1101 const QuaternionBase<T> &c0 = rotation.quat;
1102 const QuaternionBase<T> &ce = rotation.trans;
1103 const T &w0 = c0.w, &x0 = c0.x, &y0 = c0.y, &z0 = c0.z;
1104 const T &we = ce.w, &xe = ce.x, &ye = ce.y, &ze = ce.z;
1105 /* Rotation. */
1106 MatT mat = from_rotation<T, NumCol, NumRow>(c0);
1107 /* Translation. */
1108 mat[3][0] = T(2) * (-we * x0 + xe * w0 - ye * z0 + ze * y0);
1109 mat[3][1] = T(2) * (-we * y0 + xe * z0 + ye * w0 - ze * x0);
1110 mat[3][2] = T(2) * (-we * z0 - xe * y0 + ye * x0 + ze * w0);
1111 /* Scale. */
1112 if (rotation.scale_weight != T(0)) {
1113 mat.template view<4, 4>() = mat * rotation.scale;
1114 }
1115 return mat;
1116}
1117
1118template<typename T, int NumCol, int NumRow>
1120{
1121 using MatT = MatBase<T, NumCol, NumRow>;
1122 MatT mat = MatT::identity();
1123 mat.x_axis() = to_vector<VecBase<T, 3>>(rotation.axes.x);
1124 mat.y_axis() = to_vector<VecBase<T, 3>>(rotation.axes.y);
1125 mat.z_axis() = to_vector<VecBase<T, 3>>(rotation.axes.z);
1126 return mat;
1127}
1128
1129template<typename T, int NumCol, int NumRow, typename AngleT>
1131{
1132 using MatT = MatBase<T, NumCol, NumRow>;
1133 using Vec3T = typename MatT::vec3_type;
1134 const T angle_sin = sin(rotation.angle());
1135 const T angle_cos = cos(rotation.angle());
1136 const Vec3T &axis = rotation.axis();
1137
1139
1140 T ico = (T(1) - angle_cos);
1141 Vec3T nsi = axis * angle_sin;
1142
1143 Vec3T n012 = (axis * axis) * ico;
1144 T n_01 = (axis[0] * axis[1]) * ico;
1145 T n_02 = (axis[0] * axis[2]) * ico;
1146 T n_12 = (axis[1] * axis[2]) * ico;
1147
1148 MatT mat = from_scale<MatT>(n012 + angle_cos);
1149 mat[0][1] = n_01 + nsi[2];
1150 mat[0][2] = n_02 - nsi[1];
1151 mat[1][0] = n_01 - nsi[2];
1152 mat[1][2] = n_12 + nsi[0];
1153 mat[2][0] = n_02 + nsi[1];
1154 mat[2][1] = n_12 - nsi[0];
1155 return mat;
1156}
1157
1158template<typename T, int NumCol, int NumRow>
1160{
1161 using MatT = MatBase<T, NumCol, NumRow>;
1162 const T cos_i = cos(rotation);
1163 const T sin_i = sin(rotation);
1164
1165 MatT mat = MatT::identity();
1166 mat[0][0] = cos_i;
1167 mat[1][0] = -sin_i;
1168
1169 mat[0][1] = sin_i;
1170 mat[1][1] = cos_i;
1171 return mat;
1172}
1173
1174/* Using explicit template instantiations in order to reduce compilation time. */
1175extern template MatBase<float, 2, 2> from_rotation(const AngleRadian &rotation);
1176extern template MatBase<float, 3, 3> from_rotation(const AngleRadian &rotation);
1177extern template MatBase<float, 3, 3> from_rotation(const EulerXYZ &rotation);
1178extern template MatBase<float, 4, 4> from_rotation(const EulerXYZ &rotation);
1179extern template MatBase<float, 3, 3> from_rotation(const Euler3 &rotation);
1180extern template MatBase<float, 4, 4> from_rotation(const Euler3 &rotation);
1181extern template MatBase<float, 3, 3> from_rotation(const Quaternion &rotation);
1182extern template MatBase<float, 4, 4> from_rotation(const Quaternion &rotation);
1183extern template MatBase<float, 3, 3> from_rotation(const AxisAngle &rotation);
1184extern template MatBase<float, 4, 4> from_rotation(const AxisAngle &rotation);
1186extern template MatBase<float, 4, 4> from_rotation(const AxisAngleCartesian &rotation);
1187
1188} // namespace detail
1189
1190template<typename T> [[nodiscard]] inline AngleRadianBase<T> to_angle(const MatBase<T, 2, 2> &mat)
1191{
1192 return detail::normalized_to_angle(mat);
1193}
1194
1195template<typename T>
1196[[nodiscard]] inline Euler3Base<T> to_euler(const MatBase<T, 3, 3> &mat, EulerOrder order)
1197{
1198 Euler3Base<T> eul1(order), eul2(order);
1199 detail::normalized_to_eul2(mat, eul1, eul2);
1200 /* Return best, which is just the one with lowest values in it. */
1201 return (length_manhattan(VecBase<T, 3>(eul1)) > length_manhattan(VecBase<T, 3>(eul2))) ? eul2 :
1202 eul1;
1203}
1204
1205template<typename T> [[nodiscard]] inline EulerXYZBase<T> to_euler(const MatBase<T, 3, 3> &mat)
1206{
1207 EulerXYZBase<T> eul1, eul2;
1208 detail::normalized_to_eul2(mat, eul1, eul2);
1209 /* Return best, which is just the one with lowest values in it. */
1210 return (length_manhattan(VecBase<T, 3>(eul1)) > length_manhattan(VecBase<T, 3>(eul2))) ? eul2 :
1211 eul1;
1212}
1213
1214template<typename T>
1215[[nodiscard]] inline Euler3Base<T> to_euler(const MatBase<T, 4, 4> &mat, EulerOrder order)
1216{
1217 /* TODO(fclem): Avoid the copy with 3x3 ref. */
1218 return to_euler<T>(MatBase<T, 3, 3>(mat), order);
1219}
1220
1221template<typename T> [[nodiscard]] inline EulerXYZBase<T> to_euler(const MatBase<T, 4, 4> &mat)
1222{
1223 /* TODO(fclem): Avoid the copy with 3x3 ref. */
1224 return to_euler<T>(MatBase<T, 3, 3>(mat));
1225}
1226
1227template<typename T>
1228[[nodiscard]] inline Euler3Base<T> to_nearest_euler(const MatBase<T, 3, 3> &mat,
1229 const Euler3Base<T> &reference)
1230{
1231 Euler3Base<T> eul1(reference.order()), eul2(reference.order());
1232 detail::normalized_to_eul2(mat, eul1, eul2);
1233 eul1 = eul1.wrapped_around(reference);
1234 eul2 = eul2.wrapped_around(reference);
1235 /* Return best, which is just the one with lowest values it in. */
1236 return (length_manhattan(VecBase<T, 3>(eul1) - VecBase<T, 3>(reference)) >
1237 length_manhattan(VecBase<T, 3>(eul2) - VecBase<T, 3>(reference))) ?
1238 eul2 :
1239 eul1;
1240}
1241
1242template<typename T>
1243[[nodiscard]] inline EulerXYZBase<T> to_nearest_euler(const MatBase<T, 3, 3> &mat,
1244 const EulerXYZBase<T> &reference)
1245{
1246 EulerXYZBase<T> eul1, eul2;
1247 detail::normalized_to_eul2(mat, eul1, eul2);
1248 eul1 = eul1.wrapped_around(reference);
1249 eul2 = eul2.wrapped_around(reference);
1250 /* Return best, which is just the one with lowest values it in. */
1251 return (length_manhattan(VecBase<T, 3>(eul1) - VecBase<T, 3>(reference)) >
1252 length_manhattan(VecBase<T, 3>(eul2) - VecBase<T, 3>(reference))) ?
1253 eul2 :
1254 eul1;
1255}
1256
1257template<typename T>
1258[[nodiscard]] inline Euler3Base<T> to_nearest_euler(const MatBase<T, 4, 4> &mat,
1259 const Euler3Base<T> &reference)
1260{
1261 /* TODO(fclem): Avoid the copy with 3x3 ref. */
1262 return to_euler<T>(MatBase<T, 3, 3>(mat), reference);
1263}
1264
1265template<typename T>
1266[[nodiscard]] inline EulerXYZBase<T> to_nearest_euler(const MatBase<T, 4, 4> &mat,
1267 const EulerXYZBase<T> &reference)
1268{
1269 /* TODO(fclem): Avoid the copy with 3x3 ref. */
1270 return to_euler<T>(MatBase<T, 3, 3>(mat), reference);
1271}
1272
1273template<typename T>
1274[[nodiscard]] inline QuaternionBase<T> to_quaternion(const MatBase<T, 3, 3> &mat)
1275{
1277}
1278
1279template<typename T>
1280[[nodiscard]] inline QuaternionBase<T> to_quaternion(const MatBase<T, 4, 4> &mat)
1281{
1282 /* TODO(fclem): Avoid the copy with 3x3 ref. */
1284}
1285
1290template<typename T>
1292{
1293 /* Conversion to quaternion asserts when the matrix contains some kinds of shearing, conversion
1294 * to euler does not. */
1295 /* TODO: Find a better algorithm that can convert untrusted matrices to quaternions directly. */
1296 return to_quaternion(to_euler(mat));
1297}
1298
1299template<bool AllowNegativeScale, typename T, int NumCol, int NumRow>
1300[[nodiscard]] inline VecBase<T, 3> to_scale(const MatBase<T, NumCol, NumRow> &mat)
1301{
1302 VecBase<T, 3> result = {length(mat.x_axis()), length(mat.y_axis()), length(mat.z_axis())};
1303 if constexpr (AllowNegativeScale) {
1304 if (is_negative(mat)) [[unlikely]] {
1305 result = -result;
1306 }
1307 }
1308 return result;
1309}
1310
1311template<bool AllowNegativeScale, typename T>
1312[[nodiscard]] inline VecBase<T, 2> to_scale(const MatBase<T, 2, 2> &mat)
1313{
1314 VecBase<T, 2> result = {length(mat.x), length(mat.y)};
1315 if constexpr (AllowNegativeScale) {
1316 if (is_negative(mat)) [[unlikely]] {
1317 result = -result;
1318 }
1319 }
1320 return result;
1321}
1322
1323/* Implementation details. Use `to_euler` and `to_quaternion` instead. */
1324namespace detail {
1325
1326template<typename T>
1327inline void to_rotation(const MatBase<T, 2, 2> &mat, AngleRadianBase<T> &r_rotation)
1328{
1329 r_rotation = to_angle<T>(mat);
1330}
1331
1332template<typename T>
1333inline void to_rotation(const MatBase<T, 3, 3> &mat, QuaternionBase<T> &r_rotation)
1334{
1335 r_rotation = to_quaternion<T>(mat);
1336}
1337
1338template<typename T>
1339inline void to_rotation(const MatBase<T, 3, 3> &mat, EulerXYZBase<T> &r_rotation)
1340{
1341 r_rotation = to_euler<T>(mat);
1342}
1343
1344template<typename T>
1345inline void to_rotation(const MatBase<T, 3, 3> &mat, Euler3Base<T> &r_rotation)
1346{
1347 r_rotation = to_euler<T>(mat, r_rotation.order());
1348}
1349
1350} // namespace detail
1351
1352template<bool AllowNegativeScale, typename T>
1353inline void to_rot_scale(const MatBase<T, 2, 2> &mat,
1354 AngleRadianBase<T> &r_rotation,
1355 VecBase<T, 2> &r_scale)
1356{
1357 MatBase<T, 2, 2> normalized_mat = normalize_and_get_size(mat, r_scale);
1358 if constexpr (AllowNegativeScale) {
1359 if (is_negative(normalized_mat)) [[unlikely]] {
1360 normalized_mat = -normalized_mat;
1361 r_scale = -r_scale;
1362 }
1363 }
1364 detail::to_rotation<T>(normalized_mat, r_rotation);
1365}
1366
1367template<bool AllowNegativeScale, typename T>
1368inline void to_loc_rot_scale(const MatBase<T, 3, 3> &mat,
1369 VecBase<T, 2> &r_location,
1370 AngleRadianBase<T> &r_rotation,
1371 VecBase<T, 2> &r_scale)
1372{
1373 r_location = mat.location();
1374 to_rot_scale<AllowNegativeScale>(MatBase<T, 2, 2>(mat), r_rotation, r_scale);
1375}
1376
1377template<bool AllowNegativeScale, typename T, typename RotationT>
1378inline void to_rot_scale(const MatBase<T, 3, 3> &mat,
1379 RotationT &r_rotation,
1380 VecBase<T, 3> &r_scale)
1381{
1382 MatBase<T, 3, 3> normalized_mat = normalize_and_get_size(mat, r_scale);
1383 if constexpr (AllowNegativeScale) {
1384 if (is_negative(normalized_mat)) [[unlikely]] {
1385 normalized_mat = -normalized_mat;
1386 r_scale = -r_scale;
1387 }
1388 }
1389 detail::to_rotation<T>(normalized_mat, r_rotation);
1390}
1391
1392template<bool AllowNegativeScale, typename T, typename RotationT>
1393inline void to_loc_rot_scale(const MatBase<T, 4, 4> &mat,
1394 VecBase<T, 3> &r_location,
1395 RotationT &r_rotation,
1396 VecBase<T, 3> &r_scale)
1397{
1398 r_location = mat.location();
1399 to_rot_scale<AllowNegativeScale>(MatBase<T, 3, 3>(mat), r_rotation, r_scale);
1400}
1401
1406template<bool AllowNegativeScale, typename T, typename RotationT>
1408 VecBase<T, 3> &r_location,
1409 RotationT &r_rotation,
1410 VecBase<T, 3> &r_scale)
1411{
1412 EulerXYZBase<T> euler_rotation;
1413 to_loc_rot_scale<AllowNegativeScale>(mat, r_location, euler_rotation, r_scale);
1414 if constexpr (std::is_same_v<std::decay_t<RotationT>, QuaternionBase<T>>) {
1415 r_rotation = to_quaternion(euler_rotation);
1416 }
1417 else {
1418 r_rotation = RotationT(euler_rotation);
1419 }
1420}
1421
1422template<typename MatT> [[nodiscard]] MatT from_location(const typename MatT::loc_type &location)
1423{
1424 MatT mat = MatT::identity();
1425 mat.location() = location;
1426 return mat;
1427}
1428
1429template<typename MatT, int ScaleDim>
1431{
1432 BLI_STATIC_ASSERT(ScaleDim <= MatT::min_dim,
1433 "Scale dimension should fit the matrix diagonal length.");
1434 MatT result{};
1436 [&](auto i) { result[i][i] = (i < ScaleDim) ? scale[i] : typename MatT::base_type(1); });
1437 return result;
1438}
1439
1440template<typename MatT, typename RotationT>
1441[[nodiscard]] MatT from_rotation(const RotationT &rotation)
1442{
1444}
1445
1446template<typename MatT, typename RotationT, typename VectorT>
1447[[nodiscard]] MatT from_rot_scale(const RotationT &rotation, const VectorT &scale)
1448{
1449 return from_rotation<MatT>(rotation) * from_scale<MatT>(scale);
1450}
1451
1452template<typename MatT, typename RotationT, int ScaleDim>
1453[[nodiscard]] MatT from_loc_rot_scale(const typename MatT::loc_type &location,
1454 const RotationT &rotation,
1456{
1457 using MatRotT =
1459 MatT mat = MatT(from_rot_scale<MatRotT>(rotation, scale));
1460 mat.location() = location;
1461 return mat;
1462}
1463
1464template<typename MatT, typename RotationT>
1465[[nodiscard]] MatT from_loc_rot(const typename MatT::loc_type &location, const RotationT &rotation)
1466{
1467 using MatRotT =
1469 MatT mat = MatT(from_rotation<MatRotT>(rotation));
1470 mat.location() = location;
1471 return mat;
1472}
1473
1474template<typename MatT, int ScaleDim>
1475[[nodiscard]] MatT from_loc_scale(const typename MatT::loc_type &location,
1477{
1478 MatT mat = MatT(from_scale<MatT>(scale));
1479 mat.location() = location;
1480 return mat;
1481}
1482
1483template<typename T> MatBase<T, 2, 2> from_direction(const VecBase<T, 2> &direction)
1484{
1485 BLI_assert(is_unit_scale(direction));
1486 return MatBase<T, 2, 2>(direction,
1487 VecBase<T, 2>(direction.y, direction.x) * VecBase<T, 2>(-1, 1));
1488}
1489
1490template<typename MatT, typename VectorT>
1491[[nodiscard]] MatT from_orthonormal_axes(const VectorT forward, const VectorT up)
1492{
1493 BLI_assert(is_unit_scale(forward));
1495
1496 /* TODO(fclem): This is wrong. Forward is Y. */
1497 MatT matrix;
1498 matrix.x_axis() = forward;
1499 /* Beware of handedness! Blender uses right-handedness.
1500 * Resulting matrix should have determinant of 1. */
1501 matrix.y_axis() = math::cross(up, forward);
1502 matrix.z_axis() = up;
1503 return matrix;
1504}
1505
1506template<typename MatT, typename VectorT>
1507[[nodiscard]] MatT from_orthonormal_axes(const VectorT location,
1508 const VectorT forward,
1509 const VectorT up)
1510{
1512 MatT matrix = MatT(from_orthonormal_axes<Mat3x3>(forward, up));
1513 matrix.location() = location;
1514 return matrix;
1515}
1516
1517template<typename MatT, typename VectorT> [[nodiscard]] MatT from_up_axis(const VectorT up)
1518{
1520 using T = typename MatT::base_type;
1521 using Vec3T = VecBase<T, 3>;
1522 /* Duff, Tom, et al. "Building an orthonormal basis, revisited." JCGT 6.1 (2017). */
1523 T sign = up.z >= T(0) ? T(1) : T(-1);
1524 T a = T(-1) / (sign + up.z);
1525 T b = up.x * up.y * a;
1526
1527 MatBase<T, 3, 3> basis;
1528 basis.x_axis() = Vec3T(1.0f + sign * square(up.x) * a, sign * b, -sign * up.x);
1529 basis.y_axis() = Vec3T(b, sign + square(up.y) * a, -up.y);
1530 basis.z_axis() = up;
1531 return MatT(basis);
1532}
1533
1534template<typename MatT> [[nodiscard]] MatT orthogonalize(const MatT &mat, const Axis axis)
1535{
1536 using T = typename MatT::base_type;
1537 using Vec3T = VecBase<T, 3>;
1538 Vec3T scale;
1540 R.x = normalize_and_get_length(mat.x_axis(), scale.x);
1541 R.y = normalize_and_get_length(mat.y_axis(), scale.y);
1542 R.z = normalize_and_get_length(mat.z_axis(), scale.z);
1543 /* NOTE(fclem) This is a direct port from `orthogonalize_m4()`.
1544 * To select the secondary axis, it checks if the candidate axis is not colinear.
1545 * The issue is that the candidate axes are not normalized so this dot product
1546 * check is kind of pointless.
1547 * Because of this, the target axis could still be colinear but pass the check. */
1548#if 1 /* Reproduce C API behavior. Do not normalize other axes. */
1549 switch (axis) {
1550 case Axis::X:
1551 R.y = mat.y_axis();
1552 R.z = mat.z_axis();
1553 break;
1554 case Axis::Y:
1555 R.x = mat.x_axis();
1556 R.z = mat.z_axis();
1557 break;
1558 case Axis::Z:
1559 R.x = mat.x_axis();
1560 R.y = mat.y_axis();
1561 break;
1562 }
1563#endif
1564
1570 switch (axis) {
1571 case Axis::X:
1572 if (dot(R.x, R.y) < T(1)) {
1573 R.z = normalize(cross(R.x, R.y));
1574 R.y = cross(R.z, R.x);
1575 }
1576 else if (dot(R.x, R.z) < T(1)) {
1577 R.y = normalize(cross(R.z, R.x));
1578 R.z = cross(R.x, R.y);
1579 }
1580 else {
1581 R.z = normalize(cross(R.x, Vec3T(R.x.y, R.x.z, R.x.x)));
1582 R.y = cross(R.z, R.x);
1583 }
1584 break;
1585 case Axis::Y:
1586 if (dot(R.y, R.x) < T(1)) {
1587 R.z = normalize(cross(R.x, R.y));
1588 R.x = cross(R.y, R.z);
1589 }
1590 /* FIXME(fclem): THIS IS WRONG. Should be dot(R.y, R.z). Following C code for now... */
1591 else if (dot(R.x, R.z) < T(1)) {
1592 R.x = normalize(cross(R.y, R.z));
1593 R.z = cross(R.x, R.y);
1594 }
1595 else {
1596 R.x = normalize(cross(R.y, Vec3T(R.y.y, R.y.z, R.y.x)));
1597 R.z = cross(R.x, R.y);
1598 }
1599 break;
1600 case Axis::Z:
1601 if (dot(R.z, R.x) < T(1)) {
1602 R.y = normalize(cross(R.z, R.x));
1603 R.x = cross(R.y, R.z);
1604 }
1605 else if (dot(R.z, R.y) < T(1)) {
1606 R.x = normalize(cross(R.y, R.z));
1607 R.y = cross(R.z, R.x);
1608 }
1609 else {
1610 R.x = normalize(cross(Vec3T(R.z.y, R.z.z, R.z.x), R.z));
1611 R.y = cross(R.z, R.x);
1612 }
1613 break;
1614 default:
1616 break;
1617 }
1618 /* Reapply the lost scale. */
1619 R.x *= scale.x;
1620 R.y *= scale.y;
1621 R.z *= scale.z;
1622
1623 MatT result(R);
1624 result.location() = mat.location();
1625 return result;
1626}
1627
1628template<typename MatT, typename VectorT>
1629[[nodiscard]] MatT from_origin_transform(const MatT &transform, const VectorT origin)
1630{
1631 return from_location<MatT>(origin) * transform * from_location<MatT>(-origin);
1632}
1633
1634template<typename T>
1636{
1637 return mat * point;
1638}
1639
1640template<typename T>
1642{
1643 return mat.template view<2, 2>() * point + mat.location();
1644}
1645
1646template<typename T>
1648{
1649 return mat * point;
1650}
1651
1652template<typename T>
1654{
1655 return mat.template view<3, 3>() * point + mat.location();
1656}
1657
1658template<typename T>
1660{
1661 return mat * direction;
1662}
1663
1664template<typename T>
1666{
1667 return mat.template view<3, 3>() * direction;
1668}
1669
1675template<typename T>
1677 const VecBase<T, 3> &direction)
1678{
1679 const T x = direction[0];
1680 const T y = direction[1];
1681 const T z = direction[2];
1682
1683 return {x * mat[0][0] + y * mat[0][1] + z * mat[0][2],
1684 x * mat[1][0] + y * mat[1][1] + z * mat[1][2],
1685 x * mat[2][0] + y * mat[2][1] + z * mat[2][2]};
1686}
1687
1688template<typename T>
1690 const VecBase<T, 3> &direction)
1691{
1692 const T x = direction[0];
1693 const T y = direction[1];
1694 const T z = direction[2];
1695
1696 return {x * mat[0][0] + y * mat[0][1] + z * mat[0][2],
1697 x * mat[1][0] + y * mat[1][1] + z * mat[1][2],
1698 x * mat[2][0] + y * mat[2][1] + z * mat[2][2]};
1699}
1700
1701template<typename T, int N, int NumRow>
1703{
1704 VecBase<T, N + 1> tmp(point, T(1));
1705 tmp = mat * tmp;
1706 /* Absolute value to not flip the frustum upside down behind the camera. */
1707 return VecBase<T, N>(tmp) / math::abs(tmp[N]);
1708}
1709
1710template<typename T, int N, int NumRow>
1712{
1713 VecBase<T, N + 1> tmp(point, T(1));
1714 tmp = mat * tmp;
1715 /* Absolute value to not flip the frustum upside down behind the camera. */
1716 return math::safe_divide(VecBase<T, N>(tmp), math::abs(tmp[N]));
1717}
1718
1719extern template float3 transform_point(const float3x3 &mat, const float3 &point);
1720extern template float3 transform_point(const float4x4 &mat, const float3 &point);
1721extern template float3 transform_direction(const float3x3 &mat, const float3 &direction);
1722extern template float3 transform_direction(const float4x4 &mat, const float3 &direction);
1723extern template float3 project_point(const float4x4 &mat, const float3 &point);
1724extern template float2 project_point(const float3x3 &mat, const float2 &point);
1725extern template float3 project_point_safe(const float4x4 &mat, const float3 &point);
1726extern template float2 project_point_safe(const float3x3 &mat, const float2 &point);
1727
1728namespace projection {
1729
1730template<typename T>
1731MatBase<T, 4, 4> orthographic(T left, T right, T bottom, T top, T near_clip, T far_clip)
1732{
1733 const T x_delta = right - left;
1734 const T y_delta = top - bottom;
1735 const T z_delta = far_clip - near_clip;
1736
1738 if (x_delta != 0 && y_delta != 0 && z_delta != 0) {
1739 mat[0][0] = T(2.0) / x_delta;
1740 mat[3][0] = -(right + left) / x_delta;
1741 mat[1][1] = T(2.0) / y_delta;
1742 mat[3][1] = -(top + bottom) / y_delta;
1743 mat[2][2] = -T(2.0) / z_delta; /* NOTE: negate Z. */
1744 mat[3][2] = -(far_clip + near_clip) / z_delta;
1745 }
1746 return mat;
1747}
1748
1749template<typename T>
1750MatBase<T, 4, 4> orthographic_infinite(T left, T right, T bottom, T top, T near_clip)
1751{
1752 const T x_delta = right - left;
1753 const T y_delta = top - bottom;
1754
1756 if (x_delta != 0 && y_delta != 0) {
1757 mat[0][0] = T(2.0) / x_delta;
1758 mat[3][0] = -(right + left) / x_delta;
1759 mat[1][1] = T(2.0) / y_delta;
1760 mat[3][1] = -(top + bottom) / y_delta;
1761 /* Page 17. Choosing an epsilon for 32 bit floating-point precision. */
1762 constexpr float eps = 2.4e-7f;
1763 /* From "Projection Matrix Tricks" by Eric Lengyel GDC 2007.
1764 * Following same procedure as the reference but for orthographic matrix.
1765 * This avoids degenerate matrix (0 determinant). */
1766 mat[2][2] = -eps;
1767 mat[3][2] = -1.0f - eps * near_clip;
1768 }
1769 return mat;
1770}
1771
1772template<typename T>
1773MatBase<T, 4, 4> perspective(T left, T right, T bottom, T top, T near_clip, T far_clip)
1774{
1775 const T x_delta = right - left;
1776 const T y_delta = top - bottom;
1777 const T z_delta = far_clip - near_clip;
1778
1780 if (x_delta != 0 && y_delta != 0 && z_delta != 0) {
1781 mat[0][0] = near_clip * T(2.0) / x_delta;
1782 mat[1][1] = near_clip * T(2.0) / y_delta;
1783 mat[2][0] = (right + left) / x_delta; /* NOTE: negate Z. */
1784 mat[2][1] = (top + bottom) / y_delta;
1785 mat[2][2] = -(far_clip + near_clip) / z_delta;
1786 mat[2][3] = -1.0f;
1787 mat[3][2] = (-2.0f * near_clip * far_clip) / z_delta;
1788 mat[3][3] = 0.0f;
1789 }
1790 return mat;
1791}
1792
1793template<typename T>
1794MatBase<T, 4, 4> perspective_infinite(T left, T right, T bottom, T top, T near_clip)
1795{
1796 const T x_delta = right - left;
1797 const T y_delta = top - bottom;
1798
1799 /* From "Projection Matrix Tricks" by Eric Lengyel GDC 2007. */
1801 if (x_delta != 0 && y_delta != 0) {
1802 mat[0][0] = near_clip * T(2.0) / x_delta;
1803 mat[1][1] = near_clip * T(2.0) / y_delta;
1804 mat[2][0] = (right + left) / x_delta; /* NOTE: negate Z. */
1805 mat[2][1] = (top + bottom) / y_delta;
1806 /* Page 17. Choosing an epsilon for 32 bit floating-point precision. */
1807 constexpr float eps = 2.4e-7f;
1808 mat[2][2] = -1.0f;
1809 mat[2][3] = (eps - 1.0f);
1810 mat[3][2] = (eps - 2.0f) * near_clip;
1811 mat[3][3] = 0.0f;
1812 }
1813 return mat;
1814}
1815
1816template<typename T>
1818 T angle_left, T angle_right, T angle_bottom, T angle_top, T near_clip, T far_clip)
1819{
1820 MatBase<T, 4, 4> mat = perspective(math::tan(angle_left),
1821 math::tan(angle_right),
1822 math::tan(angle_bottom),
1823 math::tan(angle_top),
1824 near_clip,
1825 far_clip);
1826 mat[0][0] /= near_clip;
1827 mat[1][1] /= near_clip;
1828 return mat;
1829}
1830
1831template<typename T>
1832[[nodiscard]] MatBase<T, 4, 4> translate(const MatBase<T, 4, 4> &mat, const VecBase<T, 2> &offset)
1833{
1835 const bool is_perspective = mat[2][3] == -1.0f;
1836 const bool is_perspective_infinite = mat[2][2] == -1.0f;
1837 if (is_perspective || is_perspective_infinite) {
1838 result[2][0] -= mat[0][0] * offset.x / math::length(float3(mat[0][0], mat[1][0], mat[2][0]));
1839 result[2][1] -= mat[1][1] * offset.y / math::length(float3(mat[0][1], mat[1][1], mat[2][1]));
1840 }
1841 else {
1842 result[3][0] += offset.x;
1843 result[3][1] += offset.y;
1844 }
1845 return result;
1846}
1847
1848extern template float4x4 orthographic(
1849 float left, float right, float bottom, float top, float near_clip, float far_clip);
1850extern template float4x4 perspective(
1851 float left, float right, float bottom, float top, float near_clip, float far_clip);
1852
1853} // namespace projection
1854
1856
1863
1866 MutableSpan<float3> points,
1867 bool use_threading = true);
1869 const float4x4 &transform,
1871 bool use_threading = true);
1872
1873} // namespace blender::math
#define BLI_assert_unreachable()
SIMD_FORCE_INLINE const btScalar & z() const
Return the z value.
Definition btQuadWord.h:117
static constexpr Value Z
static constexpr Value X
static constexpr Value Y
#define unroll
#define N
#define T
#define R
QuaternionBase< T > normalized_to_quat_with_checks(const MatBase< T, 3, 3 > &mat)
void normalized_to_eul2(const MatBase< T, 3, 3 > &mat, EulerXYZBase< T > &eul1, EulerXYZBase< T > &eul2)
void to_rotation(const MatBase< T, 2, 2 > &mat, AngleRadianBase< T > &r_rotation)
QuaternionBase< T > normalized_to_quat_fast(const MatBase< T, 3, 3 > &mat)
AngleRadianBase< T > normalized_to_angle(const MatBase< T, 2, 2 > &mat)
MatBase< T, NumCol, NumRow > from_rotation(const AngleRadianBase< T > &rotation)
MatBase< T, 4, 4 > orthographic(T left, T right, T bottom, T top, T near_clip, T far_clip)
Create an orthographic projection matrix using OpenGL coordinate convention: Maps each axis range to ...
MatBase< T, 4, 4 > orthographic_infinite(T left, T right, T bottom, T top)
Create an orthographic projection matrix using OpenGL coordinate convention: Maps each axis range to ...
MatBase< T, 4, 4 > perspective_fov(T angle_left, T angle_right, T angle_bottom, T angle_top, T near_clip, T far_clip)
Create a perspective projection matrix using OpenGL coordinate convention: Maps each axis range to [-...
MatBase< T, 4, 4 > translate(const MatBase< T, 4, 4 > &mat, const VecBase< T, 2 > &offset)
Translate a projection matrix after creation in the screen plane. Usually used for anti-aliasing jitt...
MatBase< T, 4, 4 > perspective_infinite(T left, T right, T bottom, T top, T near_clip)
Create a perspective projection matrix using OpenGL coordinate convention: Maps each axis range to [-...
MatBase< T, 4, 4 > perspective(T left, T right, T bottom, T top, T near_clip, T far_clip)
Create a perspective projection matrix using OpenGL coordinate convention: Maps each axis range to [-...
MatBase< T, Size, Size > adjoint(const MatBase< T, Size, Size > &mat)
AngleRadianBase< float > AngleRadian
AngleRadianBase< T > to_angle(const MatBase< T, 2, 2 > &mat)
QuaternionBase< float > Quaternion
T length_squared(const VecBase< T, Size > &a)
T cos(const AngleRadianBase< T > &a)
MatBase< T, NumCol, NumRow > transpose(const MatBase< T, NumRow, NumCol > &mat)
VectorT project_point_safe(const MatT &mat, const VectorT &point)
VecBase< T, 3 > transform_direction_transposed(const MatBase< T, 3, 3 > &mat, const VecBase< T, 3 > &direction)
bool is_normalized(const DualQuaternionBase< T > &dq)
T sqrt(const T &a)
QuaternionBase< T > to_quaternion(const AxisAngleBase< T, AngleT > &axis_angle)
T safe_divide(const T &a, const T &b)
bool is_negative(const MatBase< T, 3, 3 > &mat)
MatBase< T, NumCol, NumRow > scale(const MatBase< T, NumCol, NumRow > &mat, const VectorT &scale)
bool is_orthonormal(const MatT &mat)
T sign(const T &a)
Quaternion to_quaternion_legacy(const float3x3 &mat)
MatT from_up_axis(const VectorT up)
T to_vector(const Axis axis)
T length(const VecBase< T, Size > &a)
MatT from_loc_rot(const typename MatT::loc_type &location, const RotationT &rotation)
MatT from_loc_scale(const typename MatT::loc_type &location, const VecBase< typename MatT::base_type, ScaleDim > &scale)
AxisAngleBase< float, AngleRadianBase< float > > AxisAngle
bool is_uniformly_scaled(const MatT &mat)
QuaternionBase< T > normalize_and_get_length(const QuaternionBase< T > &q, T &out_length)
bool is_unit_scale(const MatBase< T, NumCol, NumRow > &m)
MatBase< T, Size, Size > pseudo_invert(const MatBase< T, Size, Size > &mat, T epsilon=1e-8)
T length_manhattan(const VecBase< T, Size > &a)
EulerXYZBase< float > EulerXYZ
T dot(const QuaternionBase< T > &a, const QuaternionBase< T > &b)
bool is_zero(const T &a)
CartesianBasis invert(const CartesianBasis &basis)
AxisSigned cross(const AxisSigned a, const AxisSigned b)
VectorT project_point(const MatT &mat, const VectorT &point)
MatT from_scale(const VecBase< typename MatT::base_type, ScaleDim > &scale)
T interpolate(const T &a, const T &b, const FactorT &t)
void transform_normals(const float3x3 &transform, MutableSpan< float3 > normals)
EulerXYZBase< T > to_nearest_euler(const MatBase< T, 3, 3 > &mat, const EulerXYZBase< T > &reference)
MatBase< T, 2, 2 > from_direction(const VecBase< T, 2 > &direction)
T atan2(const T &y, const T &x)
MatBase< T, 3, 3 > interpolate_fast(const MatBase< T, 3, 3 > &a, const MatBase< T, 3, 3 > &b, T t)
MatBase< T, NumCol, NumRow > translate(const MatBase< T, NumCol, NumRow > &mat, const VectorT &translation)
QuaternionBase< T > normalized_to_quaternion_safe(const MatBase< T, 3, 3 > &mat)
MatBase< T, NumCol, NumRow > normalize_and_get_size(const MatBase< T, NumCol, NumRow > &a, VectorT &r_size)
MatT from_origin_transform(const MatT &transform, const VectorT origin)
MatBase< T, NumCol, NumRow > normalize(const MatBase< T, NumCol, NumRow > &a)
VecBase< T, 3 > to_scale(const MatBase< T, NumCol, NumRow > &mat)
AxisAngleBase< float, AngleCartesianBase< float > > AxisAngleCartesian
T square(const T &a)
bool is_equal(const MatBase< T, NumCol, NumRow > &a, const MatBase< T, NumCol, NumRow > &b, const T epsilon=T(0))
T sin(const AngleRadianBase< T > &a)
void to_rot_scale(const MatBase< T, 2, 2 > &mat, AngleRadianBase< T > &r_rotation, VecBase< T, 2 > &r_scale)
MatT orthogonalize(const MatT &mat, const Axis axis)
Euler3Base< float > Euler3
bool is_identity(const MatBase< T, NumCol, NumRow > &mat)
T abs(const T &a)
MatBase< T, NumCol, NumRow > interpolate_linear(const MatBase< T, NumCol, NumRow > &a, const MatBase< T, NumCol, NumRow > &b, T t)
T tan(const AngleRadianBase< T > &a)
void to_loc_rot_scale_safe(const MatBase< T, 4, 4 > &mat, VecBase< T, 3 > &r_location, RotationT &r_rotation, VecBase< T, 3 > &r_scale)
MatT from_location(const typename MatT::loc_type &location)
void to_loc_rot_scale(const MatBase< T, 3, 3 > &mat, VecBase< T, 2 > &r_location, AngleRadianBase< T > &r_rotation, VecBase< T, 2 > &r_scale)
VecBase< T, 3 > transform_direction(const MatBase< T, 3, 3 > &mat, const VecBase< T, 3 > &direction)
MatT from_rot_scale(const RotationT &rotation, const VectorT &scale)
MatBase< T, NumCol, NumRow > rotate(const MatBase< T, NumCol, NumRow > &mat, const RotationT &rotation)
CartesianBasis from_orthonormal_axes(const AxisSigned forward, const AxisSigned up)
MatT from_rotation(const RotationT &rotation)
T hypot(const T &y, const T &x)
T determinant(const MatBase< T, Size, Size > &mat)
Euler3Base< T > to_euler(const AxisAngleBase< T, AngleT > &axis_angle, EulerOrder order)
void transform_points(const float4x4 &transform, MutableSpan< float3 > points, bool use_threading=true)
MatT from_loc_rot_scale(const typename MatT::loc_type &location, const RotationT &rotation, const VecBase< typename MatT::base_type, ScaleDim > &scale)
VecBase< T, 3 > transform_point(const CartesianBasis &basis, const VecBase< T, 3 > &v)
bool is_orthogonal(const MatT &mat)
BLI_assert(slot->slot_subtype.map==BMO_OP_SLOT_SUBTYPE_MAP_FLT)
BLI_STATIC_ASSERT(ARRAY_SIZE(data_mode_strings)==CTX_MODE_NUM+1, "Must have a string for each context mode") const char *CTX_data_mode_string(const bContext *C)
MatBase< float, 4, 4 > float4x4
MatBase< double, 3, 3 > double3x3
VecBase< float, 2 > float2
MatBase< float, 3, 3 > float3x3
VecBase< float, 3 > float3
constexpr void unroll(Fn fn)
Definition BLI_unroll.hh:27
ATTR_WARN_UNUSED_RESULT const BMVert const BMEdge * e
const btScalar eps
Definition poly34.cpp:11
MatBase()=default
static MatBase identity()
VecBase()=default
Euler3Base wrapped_around(const Euler3Base &reference) const
const EulerOrder & order() const
EulerXYZBase wrapped_around(const EulerXYZBase &reference) const