diff --git a/source/blender/blenlib/BLI_math_base.hh b/source/blender/blenlib/BLI_math_base.hh index 6ea68ae8a68..d5279dfb3c5 100644 --- a/source/blender/blenlib/BLI_math_base.hh +++ b/source/blender/blenlib/BLI_math_base.hh @@ -142,6 +142,11 @@ template inline T atan2(const T &y, const T &x) return std::atan2(y, x); } +template inline T hypot(const T &y, const T &x) +{ + return std::hypot(y, x); +} + template)), diff --git a/source/blender/blenlib/BLI_math_matrix.hh b/source/blender/blenlib/BLI_math_matrix.hh new file mode 100644 index 00000000000..a2d5abdc58b --- /dev/null +++ b/source/blender/blenlib/BLI_math_matrix.hh @@ -0,0 +1,1166 @@ +/* SPDX-License-Identifier: GPL-2.0-or-later + * Copyright 2022 Blender Foundation. */ + +#pragma once + +/** \file + * \ingroup bli + */ + +#include "BLI_math_base.hh" +#include "BLI_math_matrix_types.hh" +#include "BLI_math_rotation_types.hh" +#include "BLI_math_vector.hh" + +namespace blender::math { + +/* -------------------------------------------------------------------- */ +/** \name Matrix Operations + * \{ */ + +/** + * Returns the inverse of a square matrix or zero matrix on failure. + * \a r_success is optional and set to true if the matrix was inverted successfully. + */ +template +[[nodiscard]] MatBase invert(const MatBase &mat, bool &r_success); + +/** + * Flip the matrix across its diagonal. Also flips dimensions for non square matrices. + */ +template +[[nodiscard]] MatBase transpose(const MatBase &mat); + +/** + * Normalize each column of the matrix individually. + */ +template +[[nodiscard]] MatBase normalize(const MatBase &a); + +/** + * Normalize each column of the matrix individually. + * Return the length of each column vector. + */ +template +[[nodiscard]] MatBase normalize_and_get_size( + const MatBase &a, VectorT &r_size); + +/** + * Returns the determinant of the matrix. + * It can be interpreted as the signed volume (or area) of the unit cube after transformation. + */ +template [[nodiscard]] T determinant(const MatBase &mat); + +/** + * Returns the adjoint of the matrix (also known as adjugate matrix). + */ +template MatBase adjoint(const MatBase &mat); + +/** + * Equivalent to `mat * from_location(translation)` but with fewer operation. + */ +template +[[nodiscard]] MatBase translate(const MatBase &mat, + const VectorT &translation); + +/** + * Equivalent to `mat * from_rotation(rotation)` but with fewer operation. + * Optimized for AxisAngle rotation on basis vector (i.e: AxisAngle({1, 0, 0}, 0.2)). + */ +template +[[nodiscard]] MatBase rotate(const MatBase &mat, + const RotationT &rotation); + +/** + * Equivalent to `mat * from_scale(scale)` but with fewer operation. + */ +template +[[nodiscard]] MatBase scale(const MatBase &mat, + const VectorT &scale); + +/** + * Interpolate each component linearly. + */ +template +[[nodiscard]] MatBase interpolate_linear(const MatBase &a, + const MatBase &b, + T t); + +/** + * A polar-decomposition-based interpolation between matrix A and matrix B. + * + * \note This code is about five times slower than the 'naive' interpolation + * (it typically remains below 2 usec on an average i74700, + * while naive implementation remains below 0.4 usec). + * However, it gives expected results even with non-uniformly scaled matrices, + * see T46418 for an example. + * + * Based on "Matrix Animation and Polar Decomposition", by Ken Shoemake & Tom Duff + * + * \param A: Input matrix which is totally effective with `t = 0.0`. + * \param B: Input matrix which is totally effective with `t = 1.0`. + * \param t: Interpolation factor. + */ +template +[[nodiscard]] MatBase interpolate(const MatBase &a, + const MatBase &b, + T t); + +/** + * Complete transform matrix interpolation, + * based on polar-decomposition-based interpolation from #interpolate. + * + * \param A: Input matrix which is totally effective with `t = 0.0`. + * \param B: Input matrix which is totally effective with `t = 1.0`. + * \param t: Interpolation factor. + */ +template +[[nodiscard]] MatBase interpolate(const MatBase &a, + const MatBase &b, + T t); + +/** + * Naive interpolation implementation, faster than polar decomposition + * + * \note This code is about five times faster than the polar decomposition. + * However, it gives un-expected results even with non-uniformly scaled matrices, + * see T46418 for an example. + * + * \param A: Input matrix which is totally effective with `t = 0.0`. + * \param B: Input matrix which is totally effective with `t = 1.0`. + * \param t: Interpolation factor. + */ +template +[[nodiscard]] MatBase interpolate_fast(const MatBase &a, + const MatBase &b, + T t); + +/** + * Naive transform matrix interpolation, + * based on naive-decomposition-based interpolation from #interpolate_fast. + * + * \note This code is about five times faster than the polar decomposition. + * However, it gives un-expected results even with non-uniformly scaled matrices, + * see T46418 for an example. + * + * \param A: Input matrix which is totally effective with `t = 0.0`. + * \param B: Input matrix which is totally effective with `t = 1.0`. + * \param t: Interpolation factor. + */ +template +[[nodiscard]] MatBase interpolate_fast(const MatBase &a, + const MatBase &b, + T t); + +/** + * Compute Moore-Penrose pseudo inverse of matrix. + * Singular values below epsilon are ignored for stability (truncated SVD). + * Gives a good enough approximation of the regular inverse matrix if the given matrix is + * non-invertible (ex: degenerate transform). + * The returned pseudo inverse matrix `A+` of input matrix `A` + * will *not* satisfy `A+ * A = Identity` + * but will satisfy `A * A+ * A = A`. + * For more detail, see https://en.wikipedia.org/wiki/Moore%E2%80%93Penrose_inverse. + */ +template +[[nodiscard]] MatBase pseudo_invert(const MatBase &mat, + T epsilon = 1e-8); + +/** \} */ + +/* -------------------------------------------------------------------- */ +/** \name Init helpers. + * \{ */ + +/** + * Create a translation only matrix. Matrix dimensions should be at least 4 col x 3 row. + */ +template [[nodiscard]] MatT from_location(const typename MatT::vec3_type &location); + +/** + * Create a matrix whose diagonal is defined by the given scale vector. + * If vector dimension is lower than matrix diagonal, the missing terms are filled with ones. + */ +template +[[nodiscard]] MatT from_scale(const vec_base &scale); + +/** + * Create a rotation only matrix. + */ +template +[[nodiscard]] MatT from_rotation(const RotationT &rotation); + +/** + * Create a transform matrix with rotation and scale applied in this order. + */ +template +[[nodiscard]] MatT from_rot_scale(const RotationT &rotation, const VectorT &scale); + +/** + * Create a transform matrix with translation and rotation applied in this order. + */ +template +[[nodiscard]] MatT from_loc_rot(const typename MatT::vec3_type &location, + const RotationT &rotation); + +/** + * Create a transform matrix with translation, rotation and scale applied in this order. + */ +template +[[nodiscard]] MatT from_loc_rot_scale(const typename MatT::vec3_type &location, + const RotationT &rotation, + const vec_base &scale); + +/** + * Create a rotation matrix from 2 basis vectors. + * The matrix determinant is given to be positive and it can be converted to other rotation types. + * \note `forward` and `up` must be normalized. + */ +template +[[nodiscard]] MatT from_orthonormal_axes(const VectorT forward, const VectorT up); + +/** + * Create a transform matrix with translation and rotation from 2 basis vectors and a translation. + * \note `forward` and `up` must be normalized. + */ +template +[[nodiscard]] MatT from_orthonormal_axes(const VectorT location, + const VectorT forward, + const VectorT up); + +/** \} */ + +/* -------------------------------------------------------------------- */ +/** \name Conversion function. + * \{ */ + +/** + * Extract euler rotation from transform matrix. + * \return the rotation with the smallest values from the potential candidates. + */ +template +[[nodiscard]] inline detail::EulerXYZ to_euler(const MatBase &mat); +template +[[nodiscard]] inline detail::EulerXYZ to_euler(const MatBase &mat); + +/** + * Extract quaternion rotation from transform matrix. + */ +template +[[nodiscard]] inline detail::Quaternion to_quaternion(const MatBase &mat); +template +[[nodiscard]] inline detail::Quaternion to_quaternion(const MatBase &mat); + +/** + * Extract the absolute 3d scale from a transform matrix. + * \tparam AllowNegativeScale if true, will compute determinant to know if matrix is negative. + * This is a costly operation so it is disabled by default. + */ +template +[[nodiscard]] inline vec_base to_scale(const MatBase &mat); + +/** + * Decompose a matrix into location, rotation, and scale components. + * \tparam AllowNegativeScale if true, will compute determinant to know if matrix is negative. + * Rotation and scale values will be flipped if it is negative. + * This is a costly operation so it is disabled by default. + */ +template +inline void to_rot_scale(const MatBase &mat, + RotationT &r_rotation, + vec_base &r_scale); +template +inline void to_loc_rot_scale(const MatBase &mat, + vec_base &r_location, + RotationT &r_rotation, + vec_base &r_scale); + +/** \} */ + +/* -------------------------------------------------------------------- */ +/** \name Transform functions. + * \{ */ + +/** + * Transform a 3d point using a 3x3 matrix (rotation & scale). + */ +template +[[nodiscard]] vec_base transform_point(const MatBase &mat, + const vec_base &point); + +/** + * Transform a 3d point using a 4x4 matrix (location & rotation & scale). + */ +template +[[nodiscard]] vec_base transform_point(const MatBase &mat, + const vec_base &point); + +/** + * Transform a 3d direction vector using a 3x3 matrix (rotation & scale). + */ +template +[[nodiscard]] vec_base transform_direction(const MatBase &mat, + const vec_base &direction); + +/** + * Transform a 3d direction vector using a 4x4 matrix (rotation & scale). + */ +template +[[nodiscard]] vec_base transform_direction(const MatBase &mat, + const vec_base &direction); + +/** + * Project a point using a matrix (location & rotation & scale & perspective divide). + */ +template +[[nodiscard]] VectorT project_point(const MatT &mat, const VectorT &point); + +/** \} */ + +/* -------------------------------------------------------------------- */ +/** \name Projection Matrices. + * \{ */ + +namespace projection { + +/** + * \brief Create an orthographic projection matrix using OpenGL coordinate convention: + * Maps each axis range to [-1..1] range for all axes. + * The resulting matrix can be used with either #project_point or #transform_point. + */ +template +[[nodiscard]] MatBase orthographic( + T left, T right, T bottom, T top, T near_clip, T far_clip); + +/** + * \brief Create a perspective projection matrix using OpenGL coordinate convention: + * Maps each axis range to [-1..1] range for all axes. + * `left`, `right`, `bottom`, `top` are frustum side distances at `z=near_clip`. + * The resulting matrix can be used with #project_point. + */ +template +[[nodiscard]] MatBase perspective( + T left, T right, T bottom, T top, T near_clip, T far_clip); + +/** + * \brief Create a perspective projection matrix using OpenGL coordinate convention: + * Maps each axis range to [-1..1] range for all axes. + * Uses field of view angles instead of plane distances. + * The resulting matrix can be used with #project_point. + */ +template +[[nodiscard]] MatBase perspective_fov( + T angle_left, T angle_right, T angle_bottom, T angle_top, T near_clip, T far_clip); + +} // namespace projection + +/** \} */ + +/* -------------------------------------------------------------------- */ +/** \name Compare / Test + * \{ */ + +/** + * Returns true if matrix has inverted handedness. + * + * \note It doesn't use determinant(mat4x4) as only the 3x3 components are needed + * when the matrix is used as a transformation to represent location/scale/rotation. + */ +template [[nodiscard]] bool is_negative(const MatBase &mat) +{ + return determinant(mat) < T(0); +} +template [[nodiscard]] bool is_negative(const MatBase &mat); + +/** + * Returns true if matrices are equal within the given epsilon. + */ +template +[[nodiscard]] inline bool is_equal(const MatBase &a, + const MatBase &b, + const T epsilon) +{ + for (int i = 0; i < NumCol; i++) { + for (int j = 0; j < NumRow; j++) { + if (math::abs(a[i][j] - b[i][j]) > epsilon) { + return false; + } + } + } + return true; +} + +/** + * Test if the X, Y and Z axes are perpendicular with each other. + */ +template [[nodiscard]] inline bool is_orthogonal(const MatT &mat) +{ + if (math::abs(math::dot(mat.x_axis(), mat.y_axis())) > 1e-5f) { + return false; + } + if (math::abs(math::dot(mat.y_axis(), mat.z_axis())) > 1e-5f) { + return false; + } + if (math::abs(math::dot(mat.z_axis(), mat.x_axis())) > 1e-5f) { + return false; + } + return true; +} + +/** + * Test if the X, Y and Z axes are perpendicular with each other and unit length. + */ +template [[nodiscard]] inline bool is_orthonormal(const MatT &mat) +{ + if (!is_orthogonal(mat)) { + return false; + } + if (math::abs(math::length_squared(mat.x_axis()) - 1) > 1e-5f) { + return false; + } + if (math::abs(math::length_squared(mat.y_axis()) - 1) > 1e-5f) { + return false; + } + if (math::abs(math::length_squared(mat.z_axis()) - 1) > 1e-5f) { + return false; + } + return true; +} + +/** + * Test if the X, Y and Z axes are perpendicular with each other and the same length. + */ +template [[nodiscard]] inline bool is_uniformly_scaled(const MatT &mat) +{ + if (!is_orthogonal(mat)) { + return false; + } + using T = typename MatT::base_type; + const T eps = 1e-7; + const T x = math::length_squared(mat.x_axis()); + const T y = math::length_squared(mat.y_axis()); + const T z = math::length_squared(mat.z_axis()); + return (math::abs(x - y) < eps) && math::abs(x - z) < eps; +} + +template +inline bool is_zero(const MatBase &mat) +{ + for (int i = 0; i < NumCol; i++) { + if (!is_zero(mat[i])) { + return false; + } + } + return true; +} + +/** \} */ + +/* -------------------------------------------------------------------- */ +/** \name Implementation. + * \{ */ + +/* Implementation details. */ +namespace detail { + +template +[[nodiscard]] MatBase from_rotation(const EulerXYZ &rotation); + +template +[[nodiscard]] MatBase from_rotation(const Quaternion &rotation); + +template +[[nodiscard]] MatBase from_rotation(const AxisAngle &rotation); + +} // namespace detail + +/* Returns true if each individual columns are unit scaled. Mainly for assert usage. */ +template +[[nodiscard]] inline bool is_unit_scale(const MatBase &m) +{ + for (int i = 0; i < NumCol; i++) { + if (!is_unit_scale(m[i])) { + return false; + } + } + return true; +} + +template +[[nodiscard]] MatBase invert(const MatBase &mat) +{ + bool success; + return invert(mat, success); +} + +template +[[nodiscard]] MatBase transpose(const MatBase &mat) +{ + MatBase result; + unroll([&](auto i) { unroll([&](auto j) { result[i][j] = mat[j][i]; }); }); + return result; +} + +template +[[nodiscard]] MatBase translate(const MatBase &mat, + const VectorT &translation) +{ + using MatT = MatBase; + BLI_STATIC_ASSERT(VectorT::type_length <= MatT::col_len - 1, + "Translation should be at least 1 column less than the matrix."); + static constexpr int location_col = MatT::col_len - 1; + /* Avoid multiplying the last row if it exists. + * Allows using non square matrices like float3x2 and saves computation. */ + using IntermediateVecT = + vec_base MatT::col_len - 1) ? (MatT::col_len - 1) : MatT::row_len>; + + MatT result = mat; + unroll([&](auto c) { + *reinterpret_cast( + &result[location_col]) += translation[c] * + *reinterpret_cast(&mat[c]); + }); + return result; +} + +template +[[nodiscard]] MatBase rotate(const MatBase &mat, + const detail::AxisAngle &rotation) +{ + using MatT = MatBase; + using Vec3T = typename MatT::vec3_type; + const T &angle_sin = rotation.angle_sin(); + const T &angle_cos = rotation.angle_cos(); + const Vec3T &axis_vec = rotation.axis(); + + MatT result = mat; + /* axis_vec is given to be normalized. */ + if (axis_vec.x == T(1)) { + unroll([&](auto c) { + result[2][c] = -angle_sin * mat[1][c] + angle_cos * mat[2][c]; + result[1][c] = angle_cos * mat[1][c] + angle_sin * mat[2][c]; + }); + } + else if (axis_vec.y == T(1)) { + unroll([&](auto c) { + result[0][c] = angle_cos * mat[0][c] - angle_sin * mat[2][c]; + result[2][c] = angle_sin * mat[0][c] + angle_cos * mat[2][c]; + }); + } + else if (axis_vec.z == T(1)) { + unroll([&](auto c) { + result[0][c] = angle_cos * mat[0][c] + angle_sin * mat[1][c]; + result[1][c] = -angle_sin * mat[0][c] + angle_cos * mat[1][c]; + }); + } + else { + /* Un-optimized case. Arbitrary */ + result *= from_rotation(rotation); + } + return result; +} + +template +[[nodiscard]] MatBase scale(const MatBase &mat, + const VectorT &scale) +{ + BLI_STATIC_ASSERT(VectorT::type_length <= NumCol, + "Scale should be less or equal to the matrix in column count."); + MatBase result = mat; + unroll([&](auto c) { result[c] *= scale[c]; }); + return result; +} + +template +[[nodiscard]] MatBase interpolate_linear(const MatBase &a, + const MatBase &b, + T t) +{ + MatBase result; + unroll([&](auto c) { result[c] = interpolate(a[c], b[c], t); }); + return result; +} + +template +[[nodiscard]] MatBase normalize( + const MatView + &a) +{ + MatBase result; + unroll([&](auto i) { result[i] = math::normalize(a[i]); }); + return result; +} + +template +[[nodiscard]] MatBase normalize_and_get_size( + const MatView + &a, + VectorT &r_size) +{ + BLI_STATIC_ASSERT(VectorT::type_length == NumCol, + "r_size dimension should be equal to matrix column count."); + MatBase result; + unroll([&](auto i) { result[i] = math::normalize_and_get_length(a[i], r_size[i]); }); + return result; +} + +template +[[nodiscard]] MatBase normalize(const MatBase &a) +{ + MatBase result; + unroll([&](auto i) { result[i] = math::normalize(a[i]); }); + return result; +} + +template +[[nodiscard]] MatBase normalize_and_get_size( + const MatBase &a, VectorT &r_size) +{ + BLI_STATIC_ASSERT(VectorT::type_length == NumCol, + "r_size dimension should be equal to matrix column count."); + MatBase result; + unroll([&](auto i) { result[i] = math::normalize_and_get_length(a[i], r_size[i]); }); + return result; +} + +namespace detail { + +template +void normalized_to_eul2(const MatBase &mat, + detail::EulerXYZ &eul1, + detail::EulerXYZ &eul2) +{ + BLI_assert(math::is_unit_scale(mat)); + + const T cy = math::hypot(mat[0][0], mat[0][1]); + if (cy > T(16) * FLT_EPSILON) { + eul1.x = math::atan2(mat[1][2], mat[2][2]); + eul1.y = math::atan2(-mat[0][2], cy); + eul1.z = math::atan2(mat[0][1], mat[0][0]); + + eul2.x = math::atan2(-mat[1][2], -mat[2][2]); + eul2.y = math::atan2(-mat[0][2], -cy); + eul2.z = math::atan2(-mat[0][1], -mat[0][0]); + } + else { + eul1.x = math::atan2(-mat[2][1], mat[1][1]); + eul1.y = math::atan2(-mat[0][2], cy); + eul1.z = 0.0f; + + eul2 = eul1; + } +} + +/* Using explicit template instantiations in order to reduce compilation time. */ +extern template void normalized_to_eul2(const float3x3 &mat, + detail::EulerXYZ &eul1, + detail::EulerXYZ &eul2); +extern template void normalized_to_eul2(const double3x3 &mat, + detail::EulerXYZ &eul1, + detail::EulerXYZ &eul2); + +template detail::Quaternion normalized_to_quat_fast(const MatBase &mat) +{ + BLI_assert(math::is_unit_scale(mat)); + /* Caller must ensure matrices aren't negative for valid results, see: T24291, T94231. */ + BLI_assert(!math::is_negative(mat)); + + detail::Quaternion q; + + /* Method outlined by Mike Day, ref: https://math.stackexchange.com/a/3183435/220949 + * with an additional `sqrtf(..)` for higher precision result. + * Removing the `sqrt` causes tests to fail unless the precision is set to 1e-6 or larger. */ + + if (mat[2][2] < 0.0f) { + if (mat[0][0] > mat[1][1]) { + const T trace = 1.0f + mat[0][0] - mat[1][1] - mat[2][2]; + T s = 2.0f * math::sqrt(trace); + if (mat[1][2] < mat[2][1]) { + /* Ensure W is non-negative for a canonical result. */ + s = -s; + } + q.y = 0.25f * s; + s = 1.0f / s; + q.x = (mat[1][2] - mat[2][1]) * s; + q.z = (mat[0][1] + mat[1][0]) * s; + q.w = (mat[2][0] + mat[0][2]) * s; + if (UNLIKELY((trace == 1.0f) && (q.x == 0.0f && q.z == 0.0f && q.w == 0.0f))) { + /* Avoids the need to normalize the degenerate case. */ + q.y = 1.0f; + } + } + else { + const T trace = 1.0f - mat[0][0] + mat[1][1] - mat[2][2]; + T s = 2.0f * math::sqrt(trace); + if (mat[2][0] < mat[0][2]) { + /* Ensure W is non-negative for a canonical result. */ + s = -s; + } + q.z = 0.25f * s; + s = 1.0f / s; + q.x = (mat[2][0] - mat[0][2]) * s; + q.y = (mat[0][1] + mat[1][0]) * s; + q.w = (mat[1][2] + mat[2][1]) * s; + if (UNLIKELY((trace == 1.0f) && (q.x == 0.0f && q.y == 0.0f && q.w == 0.0f))) { + /* Avoids the need to normalize the degenerate case. */ + q.z = 1.0f; + } + } + } + else { + if (mat[0][0] < -mat[1][1]) { + const T trace = 1.0f - mat[0][0] - mat[1][1] + mat[2][2]; + T s = 2.0f * math::sqrt(trace); + if (mat[0][1] < mat[1][0]) { + /* Ensure W is non-negative for a canonical result. */ + s = -s; + } + q.w = 0.25f * s; + s = 1.0f / s; + q.x = (mat[0][1] - mat[1][0]) * s; + q.y = (mat[2][0] + mat[0][2]) * s; + q.z = (mat[1][2] + mat[2][1]) * s; + if (UNLIKELY((trace == 1.0f) && (q.x == 0.0f && q.y == 0.0f && q.z == 0.0f))) { + /* Avoids the need to normalize the degenerate case. */ + q.w = 1.0f; + } + } + else { + /* NOTE(@campbellbarton): A zero matrix will fall through to this block, + * needed so a zero scaled matrices to return a quaternion without rotation, see: T101848. + */ + const T trace = 1.0f + mat[0][0] + mat[1][1] + mat[2][2]; + T s = 2.0f * math::sqrt(trace); + q.x = 0.25f * s; + s = 1.0f / s; + q.y = (mat[1][2] - mat[2][1]) * s; + q.z = (mat[2][0] - mat[0][2]) * s; + q.w = (mat[0][1] - mat[1][0]) * s; + if (UNLIKELY((trace == 1.0f) && (q.y == 0.0f && q.z == 0.0f && q.w == 0.0f))) { + /* Avoids the need to normalize the degenerate case. */ + q.x = 1.0f; + } + } + } + + BLI_assert(!(q.x < 0.0f)); + BLI_assert(math::is_unit_scale(vec_base(q))); + return q; +} + +template +detail::Quaternion normalized_to_quat_with_checks(const MatBase &mat) +{ + const T det = math::determinant(mat); + if (UNLIKELY(!isfinite(det))) { + return detail::Quaternion::identity(); + } + else if (UNLIKELY(det < T(0))) { + return normalized_to_quat_fast(-mat); + } + return normalized_to_quat_fast(mat); +} + +/* Using explicit template instantiations in order to reduce compilation time. */ +extern template Quaternion normalized_to_quat_with_checks(const float3x3 &mat); +extern template Quaternion normalized_to_quat_with_checks(const double3x3 &mat); + +template +MatBase from_rotation(const EulerXYZ &rotation) +{ + using MatT = MatBase; + using DoublePrecision = typename TypeTraits::DoublePrecision; + DoublePrecision ci = math::cos(rotation.x); + DoublePrecision cj = math::cos(rotation.y); + DoublePrecision ch = math::cos(rotation.z); + DoublePrecision si = math::sin(rotation.x); + DoublePrecision sj = math::sin(rotation.y); + DoublePrecision sh = math::sin(rotation.z); + DoublePrecision cc = ci * ch; + DoublePrecision cs = ci * sh; + DoublePrecision sc = si * ch; + DoublePrecision ss = si * sh; + + MatT mat; + mat[0][0] = T(cj * ch); + mat[1][0] = T(sj * sc - cs); + mat[2][0] = T(sj * cc + ss); + + mat[0][1] = T(cj * sh); + mat[1][1] = T(sj * ss + cc); + mat[2][1] = T(sj * cs - sc); + + mat[0][2] = T(-sj); + mat[1][2] = T(cj * si); + mat[2][2] = T(cj * ci); + return mat; +} + +template +MatBase from_rotation(const Quaternion &rotation) +{ + using MatT = MatBase; + using DoublePrecision = typename TypeTraits::DoublePrecision; + DoublePrecision q0 = M_SQRT2 * DoublePrecision(rotation.x); + DoublePrecision q1 = M_SQRT2 * DoublePrecision(rotation.y); + DoublePrecision q2 = M_SQRT2 * DoublePrecision(rotation.z); + DoublePrecision q3 = M_SQRT2 * DoublePrecision(rotation.w); + + DoublePrecision qda = q0 * q1; + DoublePrecision qdb = q0 * q2; + DoublePrecision qdc = q0 * q3; + DoublePrecision qaa = q1 * q1; + DoublePrecision qab = q1 * q2; + DoublePrecision qac = q1 * q3; + DoublePrecision qbb = q2 * q2; + DoublePrecision qbc = q2 * q3; + DoublePrecision qcc = q3 * q3; + + MatT mat; + mat[0][0] = T(1.0 - qbb - qcc); + mat[0][1] = T(qdc + qab); + mat[0][2] = T(-qdb + qac); + + mat[1][0] = T(-qdc + qab); + mat[1][1] = T(1.0 - qaa - qcc); + mat[1][2] = T(qda + qbc); + + mat[2][0] = T(qdb + qac); + mat[2][1] = T(-qda + qbc); + mat[2][2] = T(1.0 - qaa - qbb); + return mat; +} + +template +MatBase from_rotation(const AxisAngle &rotation) +{ + using MatT = MatBase; + using Vec3T = typename MatT::vec3_type; + const T angle_sin = rotation.angle_sin(); + const T angle_cos = rotation.angle_cos(); + const Vec3T &axis = rotation.axis(); + + BLI_assert(is_unit_scale(axis)); + + T ico = (T(1) - angle_cos); + Vec3T nsi = axis * angle_sin; + + Vec3T n012 = (axis * axis) * ico; + T n_01 = (axis[0] * axis[1]) * ico; + T n_02 = (axis[0] * axis[2]) * ico; + T n_12 = (axis[1] * axis[2]) * ico; + + MatT mat = from_scale(n012 + angle_cos); + mat[0][1] = n_01 + nsi[2]; + mat[0][2] = n_02 - nsi[1]; + mat[1][0] = n_01 - nsi[2]; + mat[1][2] = n_12 + nsi[0]; + mat[2][0] = n_02 + nsi[1]; + mat[2][1] = n_12 - nsi[0]; + return mat; +} + +/* Using explicit template instantiations in order to reduce compilation time. */ +extern template MatBase from_rotation(const EulerXYZ &rotation); +extern template MatBase from_rotation(const EulerXYZ &rotation); +extern template MatBase from_rotation(const Quaternion &rotation); +extern template MatBase from_rotation(const Quaternion &rotation); +extern template MatBase from_rotation(const AxisAngle &rotation); +extern template MatBase from_rotation(const AxisAngle &rotation); + +} // namespace detail + +template +[[nodiscard]] inline detail::EulerXYZ to_euler(const MatBase &mat) +{ + detail::EulerXYZ eul1, eul2; + if constexpr (Normalized) { + detail::normalized_to_eul2(mat, eul1, eul2); + } + else { + detail::normalized_to_eul2(normalize(mat), eul1, eul2); + } + /* Return best, which is just the one with lowest values it in. */ + return (length_manhattan(vec_base(eul1)) > length_manhattan(vec_base(eul2))) ? eul2 : + eul1; +} + +template +[[nodiscard]] inline detail::EulerXYZ to_euler(const MatBase &mat) +{ + /* TODO(fclem): Avoid the copy with 3x3 ref. */ + return to_euler(MatBase(mat)); +} + +template +[[nodiscard]] inline detail::Quaternion to_quaternion(const MatBase &mat) +{ + using namespace math; + if constexpr (Normalized) { + return detail::normalized_to_quat_with_checks(mat); + } + else { + return detail::normalized_to_quat_with_checks(normalize(mat)); + } +} + +template +[[nodiscard]] inline detail::Quaternion to_quaternion(const MatBase &mat) +{ + /* TODO(fclem): Avoid the copy with 3x3 ref. */ + return to_quaternion(MatBase(mat)); +} + +template +[[nodiscard]] inline vec_base to_scale(const MatBase &mat) +{ + vec_base result = {length(mat.x_axis()), length(mat.y_axis()), length(mat.z_axis())}; + if constexpr (AllowNegativeScale) { + if (UNLIKELY(is_negative(mat))) { + result = -result; + } + } + return result; +} + +/* Implementation details. Use `to_euler` and `to_quaternion` instead. */ +namespace detail { + +template +inline void to_rotation(const MatBase &mat, detail::Quaternion &r_rotation) +{ + r_rotation = to_quaternion(mat); +} + +template +inline void to_rotation(const MatBase &mat, detail::EulerXYZ &r_rotation) +{ + r_rotation = to_euler(mat); +} + +} // namespace detail + +template +inline void to_rot_scale(const MatBase &mat, + RotationT &r_rotation, + vec_base &r_scale) +{ + MatBase normalized_mat = normalize_and_get_size(mat, r_scale); + if constexpr (AllowNegativeScale) { + if (UNLIKELY(is_negative(normalized_mat))) { + normalized_mat = -normalized_mat; + r_scale = -r_scale; + } + } + detail::to_rotation(normalized_mat, r_rotation); +} + +template +inline void to_loc_rot_scale(const MatBase &mat, + vec_base &r_location, + RotationT &r_rotation, + vec_base &r_scale) +{ + r_location = mat.location(); + to_rot_scale(MatBase(mat), r_rotation, r_scale); +} + +template [[nodiscard]] MatT from_location(const typename MatT::vec3_type &location) +{ + MatT mat = MatT::identity(); + mat.location() = location; + return mat; +} + +template +[[nodiscard]] MatT from_scale(const vec_base &scale) +{ + BLI_STATIC_ASSERT(ScaleDim <= MatT::min_dim, + "Scale dimension should fit the matrix diagonal length."); + MatT result{}; + unroll( + [&](auto i) { result[i][i] = (i < ScaleDim) ? scale[i] : typename MatT::base_type(1); }); + return result; +} + +template +[[nodiscard]] MatT from_rotation(const RotationT &rotation) +{ + return detail::from_rotation(rotation); +} + +template +[[nodiscard]] MatT from_rot_scale(const RotationT &rotation, const VectorT &scale) +{ + return from_rotation(rotation) * from_scale(scale); +} + +template +[[nodiscard]] MatT from_loc_rot_scale(const typename MatT::vec3_type &location, + const RotationT &rotation, + const vec_base &scale) +{ + using Mat3x3 = MatBase; + MatT mat = MatT(from_rot_scale(rotation, scale)); + mat.location() = location; + return mat; +} + +template +[[nodiscard]] MatT from_loc_rot(const typename MatT::vec3_type &location, + const RotationT &rotation) +{ + using Mat3x3 = MatBase; + MatT mat = MatT(from_rotation(rotation)); + mat.location() = location; + return mat; +} + +template +[[nodiscard]] MatT from_orthonormal_axes(const VectorT forward, const VectorT up) +{ + BLI_assert(is_unit_scale(forward)); + BLI_assert(is_unit_scale(up)); + + MatT matrix; + matrix.x_axis() = forward; + /* Beware of handedness! Blender uses right-handedness. + * Resulting matrix should have determinant of 1. */ + matrix.y_axis() = math::cross(up, forward); + matrix.z_axis() = up; + return matrix; +} + +template +[[nodiscard]] MatT from_orthonormal_axes(const VectorT location, + const VectorT forward, + const VectorT up) +{ + using Mat3x3 = MatBase; + MatT matrix = MatT(from_orthonormal_axes(forward, up)); + matrix.location() = location; + return matrix; +} + +template +vec_base transform_point(const MatBase &mat, const vec_base &point) +{ + return mat * point; +} + +template +vec_base transform_point(const MatBase &mat, const vec_base &point) +{ + return mat.template view<3, 3>() * point + mat.location(); +} + +template +vec_base transform_direction(const MatBase &mat, const vec_base &direction) +{ + return mat * direction; +} + +template +vec_base transform_direction(const MatBase &mat, const vec_base &direction) +{ + return mat.template view<3, 3>() * direction; +} + +template +vec_base project_point(const MatBase &mat, const vec_base &point) +{ + vec_base tmp(point, T(1)); + tmp = mat * tmp; + /* Absolute value to not flip the frustum upside down behind the camera. */ + return vec_base(tmp) / math::abs(tmp[N]); +} + +extern template float3 transform_point(const float3x3 &mat, const float3 &point); +extern template float3 transform_point(const float4x4 &mat, const float3 &point); +extern template float3 transform_direction(const float3x3 &mat, const float3 &direction); +extern template float3 transform_direction(const float4x4 &mat, const float3 &direction); +extern template float3 project_point(const float4x4 &mat, const float3 &point); +extern template float2 project_point(const float3x3 &mat, const float2 &point); + +namespace projection { + +template +MatBase orthographic(T left, T right, T bottom, T top, T near_clip, T far_clip) +{ + const T x_delta = right - left; + const T y_delta = top - bottom; + const T z_delta = far_clip - near_clip; + + MatBase mat = MatBase::identity(); + if (x_delta != 0 && y_delta != 0 && z_delta != 0) { + mat[0][0] = T(2.0) / x_delta; + mat[3][0] = -(right + left) / x_delta; + mat[1][1] = T(2.0) / y_delta; + mat[3][1] = -(top + bottom) / y_delta; + mat[2][2] = -T(2.0) / z_delta; /* NOTE: negate Z. */ + mat[3][2] = -(far_clip + near_clip) / z_delta; + } + return mat; +} + +template +MatBase perspective(T left, T right, T bottom, T top, T near_clip, T far_clip) +{ + const T x_delta = right - left; + const T y_delta = top - bottom; + const T z_delta = far_clip - near_clip; + + MatBase mat = MatBase::identity(); + if (x_delta != 0 && y_delta != 0 && z_delta != 0) { + mat[0][0] = near_clip * T(2.0) / x_delta; + mat[1][1] = near_clip * T(2.0) / y_delta; + mat[2][0] = (right + left) / x_delta; /* NOTE: negate Z. */ + mat[2][1] = (top + bottom) / y_delta; + mat[2][2] = -(far_clip + near_clip) / z_delta; + mat[2][3] = -1.0f; + mat[3][2] = (-2.0f * near_clip * far_clip) / z_delta; + } + return mat; +} + +template +[[nodiscard]] MatBase perspective_fov( + T angle_left, T angle_right, T angle_bottom, T angle_top, T near_clip, T far_clip) +{ + MatBase mat = perspective(math::tan(angle_left), + math::tan(angle_right), + math::tan(angle_bottom), + math::tan(angle_top), + near_clip, + far_clip); + mat[0][0] /= near_clip; + mat[1][1] /= near_clip; + return mat; +} + +extern template float4x4 orthographic( + float left, float right, float bottom, float top, float near_clip, float far_clip); +extern template float4x4 perspective( + float left, float right, float bottom, float top, float near_clip, float far_clip); + +} // namespace projection + +/** \} */ + +} // namespace blender::math diff --git a/source/blender/blenlib/BLI_math_matrix_types.hh b/source/blender/blenlib/BLI_math_matrix_types.hh new file mode 100644 index 00000000000..6d6e738fc4b --- /dev/null +++ b/source/blender/blenlib/BLI_math_matrix_types.hh @@ -0,0 +1,948 @@ +/* SPDX-License-Identifier: GPL-2.0-or-later + * Copyright 2022 Blender Foundation. */ + +#pragma once + +/** \file + * \ingroup bli + * + * Template for matrix types. + * + * The `blender::MatBase` is a Row x Col matrix (in mathematical notation) laid + * out as column major in memory. + * + * This class overloads `+, -, *` and `+=, -=, *=` mathematical operators. + * They are all using per component operation, except for a few: + * `MatBase * Vector` the vector product with the matrix. + * `Vector * MatBase` the vector product with the **transposed** matrix. + * `MatBase * MatBase` and `MatBase *= MatBase` the matrix multiplication. + * + * The `blender::MatView` allows working on a subset of a matrix without having to move the data + * around. It can be obtained using the `MatBase.view()`. It is const by default if + * the matrix type is. Otherwise, a `blender::MutableMatView` is returned. + * + * A `blender::MutableMatView`. It is mostly the same as `blender::MatView`, but can to be + * modified. + * + * This allow working with any number type `T` (float, double, mpq, ...) and to use these types in + * shared shader files (code compiled in both C++ and Shader language). To this end, only low level + * constructors are defined inside the class itself and every function working on matrices are + * defined outside of the class in the `blender::math` namespace. + */ + +#include +#include +#include +#include + +#include "BLI_math_vec_types.hh" +#include "BLI_utildefines.h" +#include "BLI_utility_mixins.hh" + +namespace blender { + +template +struct MatView; + +template +struct MutableMatView; + +template< + /* Number type. */ + typename T, + /* Number of column in the matrix. */ + int NumCol, + /* Number of row in the matrix. */ + int NumRow, + /* Alignment in bytes. Do not align matrices whose size is not a multiple of 4 component. + * This is in order to avoid padding when using arrays of matrices. */ + int Alignment = (((NumCol * NumRow) % 4) ? 4 : 0) * sizeof(T)> +struct alignas(Alignment) MatBase : public vec_struct_base, NumCol> { + + using base_type = T; + using vec3_type = vec_base; + using col_type = vec_base; + using row_type = vec_base; + static constexpr int min_dim = (NumRow < NumCol) ? NumRow : NumCol; + static constexpr int col_len = NumCol; + static constexpr int row_len = NumRow; + + MatBase() = default; + +/* Workaround issue with template BLI_ENABLE_IF((Size == 2)) not working. */ +#define BLI_ENABLE_IF_MAT(_size, _test) int S = _size, BLI_ENABLE_IF((S _test)) + + template MatBase(col_type _x, col_type _y) + { + (*this)[0] = _x; + (*this)[1] = _y; + } + + template MatBase(col_type _x, col_type _y, col_type _z) + { + (*this)[0] = _x; + (*this)[1] = _y; + (*this)[2] = _z; + } + + template + MatBase(col_type _x, col_type _y, col_type _z, col_type _w) + { + (*this)[0] = _x; + (*this)[1] = _y; + (*this)[2] = _z; + (*this)[3] = _w; + } + + /** Masking. */ + + template + explicit MatBase(const MatBase &other) + { + if constexpr ((OtherNumRow >= NumRow) && (OtherNumCol >= NumCol)) { + unroll([&](auto i) { (*this)[i] = col_type(other[i]); }); + } + else { + /* Allow enlarging following GLSL standard (i.e: mat4x4(mat3x3())). */ + unroll([&](auto i) { + unroll([&](auto j) { + if (i < OtherNumCol && j < OtherNumRow) { + (*this)[i][j] = other[i][j]; + } + else if (i == j) { + (*this)[i][j] = T(1); + } + else { + (*this)[i][j] = T(0); + } + }); + }); + } + } + +#undef BLI_ENABLE_IF_MAT + + /** Conversion from pointers (from C-style vectors). */ + + explicit MatBase(const T *ptr) + { + unroll([&](auto i) { (*this)[i] = reinterpret_cast(ptr)[i]; }); + } + + template))> explicit MatBase(const U *ptr) + { + unroll([&](auto i) { (*this)[i] = ptr[i]; }); + } + + explicit MatBase(const T (*ptr)[NumCol]) : MatBase(static_cast(ptr[0])) + { + } + + /** Conversion from other matrix types. */ + + template explicit MatBase(const MatBase &vec) + { + unroll([&](auto i) { (*this)[i] = col_type(vec[i]); }); + } + + /** C-style pointer dereference. */ + + using c_style_mat = T[NumCol][NumRow]; + + /** \note Prevent implicit cast to types that could fit other pointer constructor. */ + const c_style_mat &ptr() const + { + return *reinterpret_cast(this); + } + + /** \note Prevent implicit cast to types that could fit other pointer constructor. */ + c_style_mat &ptr() + { + return *reinterpret_cast(this); + } + + /** \note Prevent implicit cast to types that could fit other pointer constructor. */ + const T *base_ptr() const + { + return reinterpret_cast(this); + } + + /** \note Prevent implicit cast to types that could fit other pointer constructor. */ + T *base_ptr() + { + return reinterpret_cast(this); + } + + /** View creation. */ + + template + const MatView + view() const + { + return MatView( + const_cast(*this)); + } + + template + MutableMatView + view() + { + return MutableMatView(*this); + } + + /** Array access. */ + + const col_type &operator[](int index) const + { + BLI_assert(index >= 0); + BLI_assert(index < NumCol); + return reinterpret_cast(this)[index]; + } + + col_type &operator[](int index) + { + BLI_assert(index >= 0); + BLI_assert(index < NumCol); + return reinterpret_cast(this)[index]; + } + + /** Access helpers. Using Blender coordinate system. */ + + vec3_type &x_axis() + { + BLI_STATIC_ASSERT(NumCol >= 1, "Wrong Matrix dimension"); + BLI_STATIC_ASSERT(NumRow >= 3, "Wrong Matrix dimension"); + return *reinterpret_cast(&(*this)[0]); + } + + vec3_type &y_axis() + { + BLI_STATIC_ASSERT(NumCol >= 2, "Wrong Matrix dimension"); + BLI_STATIC_ASSERT(NumRow >= 3, "Wrong Matrix dimension"); + return *reinterpret_cast(&(*this)[1]); + } + + vec3_type &z_axis() + { + BLI_STATIC_ASSERT(NumCol >= 3, "Wrong Matrix dimension"); + BLI_STATIC_ASSERT(NumRow >= 3, "Wrong Matrix dimension"); + return *reinterpret_cast(&(*this)[2]); + } + + vec3_type &location() + { + BLI_STATIC_ASSERT(NumCol >= 4, "Wrong Matrix dimension"); + BLI_STATIC_ASSERT(NumRow >= 3, "Wrong Matrix dimension"); + return *reinterpret_cast(&(*this)[3]); + } + + const vec3_type &x_axis() const + { + BLI_STATIC_ASSERT(NumCol >= 1, "Wrong Matrix dimension"); + BLI_STATIC_ASSERT(NumRow >= 3, "Wrong Matrix dimension"); + return *reinterpret_cast(&(*this)[0]); + } + + const vec3_type &y_axis() const + { + BLI_STATIC_ASSERT(NumCol >= 2, "Wrong Matrix dimension"); + BLI_STATIC_ASSERT(NumRow >= 3, "Wrong Matrix dimension"); + return *reinterpret_cast(&(*this)[1]); + } + + const vec3_type &z_axis() const + { + BLI_STATIC_ASSERT(NumCol >= 3, "Wrong Matrix dimension"); + BLI_STATIC_ASSERT(NumRow >= 3, "Wrong Matrix dimension"); + return *reinterpret_cast(&(*this)[2]); + } + + const vec3_type &location() const + { + BLI_STATIC_ASSERT(NumCol >= 4, "Wrong Matrix dimension"); + BLI_STATIC_ASSERT(NumRow >= 3, "Wrong Matrix dimension"); + return *reinterpret_cast(&(*this)[3]); + } + + /** Matrix operators. */ + + friend MatBase operator+(const MatBase &a, const MatBase &b) + { + MatBase result; + unroll([&](auto i) { result[i] = a[i] + b[i]; }); + return result; + } + + friend MatBase operator+(const MatBase &a, T b) + { + MatBase result; + unroll([&](auto i) { result[i] = a[i] + b; }); + return result; + } + + friend MatBase operator+(T a, const MatBase &b) + { + return b + a; + } + + MatBase &operator+=(const MatBase &b) + { + unroll([&](auto i) { (*this)[i] += b[i]; }); + return *this; + } + + MatBase &operator+=(T b) + { + unroll([&](auto i) { (*this)[i] += b; }); + return *this; + } + + friend MatBase operator-(const MatBase &a) + { + MatBase result; + unroll([&](auto i) { result[i] = -a[i]; }); + return result; + } + + friend MatBase operator-(const MatBase &a, const MatBase &b) + { + MatBase result; + unroll([&](auto i) { result[i] = a[i] - b[i]; }); + return result; + } + + friend MatBase operator-(const MatBase &a, T b) + { + MatBase result; + unroll([&](auto i) { result[i] = a[i] - b; }); + return result; + } + + friend MatBase operator-(T a, const MatBase &b) + { + MatBase result; + unroll([&](auto i) { result[i] = a - b[i]; }); + return result; + } + + MatBase &operator-=(const MatBase &b) + { + unroll([&](auto i) { (*this)[i] -= b[i]; }); + return *this; + } + + MatBase &operator-=(T b) + { + unroll([&](auto i) { (*this)[i] -= b; }); + return *this; + } + + /** Multiply two matrices using matrix multiplication. */ + MatBase operator*(const MatBase &b) const + { + const MatBase &a = *this; + /* This is the reference implementation. + * Might be overloaded with vectorized / optimized code. */ + /* TODO(fclem): It should be possible to return non-square matrices when multiplying against + * MatBase. */ + MatBase result{}; + unroll([&](auto j) { + unroll([&](auto i) { + /* Same as dot product, but avoid dependency on vector math. */ + unroll([&](auto k) { result[j][i] += a[k][i] * b[j][k]; }); + }); + }); + return result; + } + + /** Multiply each component by a scalar. */ + friend MatBase operator*(const MatBase &a, T b) + { + MatBase result; + unroll([&](auto i) { result[i] = a[i] * b; }); + return result; + } + + /** Multiply each component by a scalar. */ + friend MatBase operator*(T a, const MatBase &b) + { + return b * a; + } + + /** Multiply two matrices using matrix multiplication. */ + MatBase &operator*=(const MatBase &b) + { + const MatBase &a = *this; + *this = a * b; + return *this; + } + + /** Multiply each component by a scalar. */ + MatBase &operator*=(T b) + { + unroll([&](auto i) { (*this)[i] *= b; }); + return *this; + } + + /** Vector operators. */ + + friend col_type operator*(const MatBase &a, const row_type &b) + { + /* This is the reference implementation. + * Might be overloaded with vectorized / optimized code. */ + col_type result(0); + unroll([&](auto c) { result += b[c] * a[c]; }); + return result; + } + + /** Multiply by the transposed. */ + friend row_type operator*(const col_type &a, const MatBase &b) + { + /* This is the reference implementation. + * Might be overloaded with vectorized / optimized code. */ + row_type result(0); + unroll([&](auto c) { unroll([&](auto r) { result[c] += b[c][r] * a[r]; }); }); + return result; + } + + /** Compare. */ + + friend bool operator==(const MatBase &a, const MatBase &b) + { + for (int i = 0; i < NumCol; i++) { + if (a[i] != b[i]) { + return false; + } + } + return true; + } + + friend bool operator!=(const MatBase &a, const MatBase &b) + { + return !(a == b); + } + + /** Miscellaneous. */ + + static MatBase diagonal(T value) + { + MatBase result{}; + unroll([&](auto i) { result[i][i] = value; }); + return result; + } + + static MatBase all(T value) + { + MatBase result; + unroll([&](auto i) { result[i] = col_type(value); }); + return result; + } + + static MatBase identity() + { + return diagonal(1); + } + + static MatBase zero() + { + return all(0); + } + + uint64_t hash() const + { + uint64_t h = 435109; + unroll([&](auto i) { + T value = (static_cast(this))[i]; + h = h * 33 + *reinterpret_cast *>(&value); + }); + return h; + } + + friend std::ostream &operator<<(std::ostream &stream, const MatBase &mat) + { + stream << "(\n"; + unroll([&](auto i) { + stream << "("; + unroll([&](auto j) { + /** NOTE: j and i are swapped to follow mathematical convention. */ + stream << mat[j][i]; + if (j < NumRow - 1) { + stream << ", "; + } + }); + stream << ")"; + if (i < NumCol - 1) { + stream << ","; + } + stream << "\n"; + }); + stream << ")\n"; + return stream; + } +}; + +template +struct MatView : NonCopyable, NonMovable { + using MatT = MatBase; + using SrcMatT = MatBase; + using col_type = vec_base; + using row_type = vec_base; + + const SrcMatT &mat; + + MatView() = delete; + + MatView(const SrcMatT &src) : mat(src) + { + BLI_STATIC_ASSERT(SrcStartCol >= 0, "View does not fit source matrix dimensions"); + BLI_STATIC_ASSERT(SrcStartRow >= 0, "View does not fit source matrix dimensions"); + BLI_STATIC_ASSERT(SrcStartCol + NumCol <= SrcNumCol, + "View does not fit source matrix dimensions"); + BLI_STATIC_ASSERT(SrcStartRow + NumRow <= SrcNumRow, + "View does not fit source matrix dimensions"); + } + + /** Allow wrapping C-style matrices using view. IMPORTANT: Alignment of src needs to match. */ + explicit MatView(const float (*src)[SrcNumRow]) + : MatView(*reinterpret_cast(&src[0][0])){}; + + /** Array access. */ + + const col_type &operator[](int index) const + { + BLI_assert(index >= 0); + BLI_assert(index < NumCol); + return *reinterpret_cast(&mat[index + SrcStartCol][SrcStartRow]); + } + + /** Conversion back to matrix. */ + + operator MatT() const + { + MatT mat; + unroll([&](auto c) { mat[c] = (*this)[c]; }); + return mat; + } + + /** Matrix operators. */ + + friend MatT operator+(const MatView &a, T b) + { + MatT result; + unroll([&](auto i) { result[i] = a[i] + b; }); + return result; + } + + friend MatT operator+(T a, const MatView &b) + { + return b + a; + } + + friend MatT operator-(const MatView &a) + { + MatT result; + unroll([&](auto i) { result[i] = -a[i]; }); + return result; + } + + template + friend MatT operator-(const MatView &a, + const MatView &b) + { + MatT result; + unroll([&](auto i) { result[i] = a[i] - b[i]; }); + return result; + } + + friend MatT operator-(const MatView &a, const MatT &b) + { + return a - b.template view(); + } + + template + friend MatT operator-(const MatView &a, + const MatView &b) + { + MatT result; + unroll([&](auto i) { result[i] = a[i] - b[i]; }); + return result; + } + + friend MatT operator-(const MatT &a, const MatView &b) + { + return a.template view() - b; + } + + friend MatT operator-(const MatView &a, T b) + { + MatT result; + unroll([&](auto i) { result[i] = a[i] - b; }); + return result; + } + + friend MatView operator-(T a, const MatView &b) + { + MatView result; + unroll([&](auto i) { result[i] = a - b[i]; }); + return result; + } + + /** Multiply two matrices using matrix multiplication. */ + template + MatBase operator*(const MatView &b) const + { + const MatView &a = *this; + /* This is the reference implementation. + * Might be overloaded with vectorized / optimized code. */ + MatBase result{}; + unroll([&](auto j) { + unroll([&](auto i) { + /* Same as dot product, but avoid dependency on vector math. */ + unroll([&](auto k) { result[j][i] += a[k][i] * b[j][k]; }); + }); + }); + return result; + } + + MatT operator*(const MatT &b) const + { + return *this * b.template view(); + } + + /** Multiply each component by a scalar. */ + friend MatT operator*(const MatView &a, T b) + { + MatT result; + unroll([&](auto i) { result[i] = a[i] * b; }); + return result; + } + + /** Multiply each component by a scalar. */ + friend MatT operator*(T a, const MatView &b) + { + return b * a; + } + + /** Vector operators. */ + + friend col_type operator*(const MatView &a, const row_type &b) + { + /* This is the reference implementation. + * Might be overloaded with vectorized / optimized code. */ + col_type result(0); + unroll([&](auto c) { result += b[c] * a[c]; }); + return result; + } + + /** Multiply by the transposed. */ + friend row_type operator*(const col_type &a, const MatView &b) + { + /* This is the reference implementation. + * Might be overloaded with vectorized / optimized code. */ + row_type result(0); + unroll([&](auto c) { unroll([&](auto r) { result[c] += b[c][r] * a[r]; }); }); + return result; + } + + /** Compare. */ + + friend bool operator==(const MatView &a, const MatView &b) + { + for (int i = 0; i < NumCol; i++) { + if (a[i] != b[i]) { + return false; + } + } + return true; + } + + friend bool operator!=(const MatView &a, const MatView &b) + { + return !(a == b); + } + + /** Miscellaneous. */ + + friend std::ostream &operator<<(std::ostream &stream, const MatView &mat) + { + return stream << mat->mat; + } +}; + +template +struct MutableMatView + : MatView { + + using MatT = MatBase; + using MatViewT = + MatView; + using SrcMatT = MatBase; + using col_type = vec_base; + using row_type = vec_base; + + public: + MutableMatView() = delete; + + MutableMatView(SrcMatT &src) : MatViewT(const_cast(src)){}; + + /** Allow wrapping C-style matrices using view. IMPORTANT: Alignment of src needs to match. */ + explicit MutableMatView(float src[SrcNumCol][SrcNumRow]) + : MutableMatView(*reinterpret_cast(&src[0][0])){}; + + /** Array access. */ + + col_type &operator[](int index) + { + return const_cast(static_cast(*this)[index]); + } + + /** Conversion to immutable view. */ + + operator MatViewT() const + { + return MatViewT(this->mat); + } + + /** Copy Assignment. */ + + template + MutableMatView &operator=(const MatView &other) + { + BLI_assert_msg( + (reinterpret_cast(&other.mat[0][0]) != + reinterpret_cast(&this->mat[0][0])) || + /* Make sure assignment won't overwrite the source. OtherSrc* is the source. */ + ((OtherSrcStartCol > SrcStartCol) || (OtherSrcStartCol + NumCol <= SrcStartCol) || + (OtherSrcStartRow > SrcStartRow + NumRow) || + (OtherSrcStartRow + NumRow <= SrcStartRow)), + "Operation is undefined if views overlap."); + unroll([&](auto i) { (*this)[i] = other[i]; }); + return *this; + } + + MutableMatView &operator=(const MatT &other) + { + *this = other.template view(); + return *this; + } + + /** Matrix operators. */ + + template + MutableMatView &operator+=(const MatView &b) + { + unroll([&](auto i) { (*this)[i] += b[i]; }); + return *this; + } + + MutableMatView &operator+=(const MatT &b) + { + return *this += b.template view(); + } + + MutableMatView &operator+=(T b) + { + unroll([&](auto i) { (*this)[i] += b; }); + return *this; + } + + template + MutableMatView &operator-=(const MatView &b) + { + unroll([&](auto i) { (*this)[i] -= b[i]; }); + return *this; + } + + MutableMatView &operator-=(const MatT &b) + { + return *this -= b.template view(); + } + + MutableMatView &operator-=(T b) + { + unroll([&](auto i) { (*this)[i] -= b; }); + return *this; + } + + /** Multiply two matrices using matrix multiplication. */ + template + MutableMatView &operator*=(const MatView &b) + { + *this = *static_cast(this) * b; + return *this; + } + + MutableMatView &operator*=(const MatT &b) + { + return *this *= b.template view(); + } + + /** Multiply each component by a scalar. */ + MutableMatView &operator*=(T b) + { + unroll([&](auto i) { (*this)[i] *= b; }); + return *this; + } +}; + +using float2x2 = MatBase; +using float2x3 = MatBase; +using float2x4 = MatBase; +using float3x2 = MatBase; +using float3x3 = MatBase; +using float3x4 = MatBase; +using float4x2 = MatBase; +using float4x3 = MatBase; +using float4x4 = MatBase; + +/* These types are reserved to wrap C matrices without copy. Note the un-alignment. */ +/* TODO: It would be preferable to align all C matrices inside DNA structs. */ +using float4x4_view = MatView; +using float4x4_mutableview = MutableMatView; + +using double2x2 = MatBase; +using double2x3 = MatBase; +using double2x4 = MatBase; +using double3x2 = MatBase; +using double3x3 = MatBase; +using double3x4 = MatBase; +using double4x2 = MatBase; +using double4x3 = MatBase; +using double4x4 = MatBase; + +/* Specialization for SSE optimization. */ +template<> float4x4 float4x4::operator*(const float4x4 &b) const; +template<> float3x3 float3x3::operator*(const float3x3 &b) const; + +extern template float2x2 float2x2::operator*(const float2x2 &b) const; +extern template double2x2 double2x2::operator*(const double2x2 &b) const; +extern template double3x3 double3x3::operator*(const double3x3 &b) const; +extern template double4x4 double4x4::operator*(const double4x4 &b) const; + +} // namespace blender diff --git a/source/blender/blenlib/BLI_math_rotation.hh b/source/blender/blenlib/BLI_math_rotation.hh new file mode 100644 index 00000000000..78c8dcbef08 --- /dev/null +++ b/source/blender/blenlib/BLI_math_rotation.hh @@ -0,0 +1,242 @@ +/* SPDX-License-Identifier: GPL-2.0-or-later */ + +#pragma once + +/** \file + * \ingroup bli + */ + +#include "BLI_math_matrix.hh" +#include "BLI_math_rotation_types.hh" +#include "BLI_math_vector.hh" + +namespace blender::math { + +/** + * Generic function for implementing slerp + * (quaternions and spherical vector coords). + * + * \param t: factor in [0..1] + * \param cosom: dot product from normalized vectors/quats. + * \param r_w: calculated weights. + */ +template inline vec_base interpolate_dot_slerp(const T t, const T cosom) +{ + const T eps = T(1e-4); + + BLI_assert(IN_RANGE_INCL(cosom, T(-1.0001), T(1.0001))); + + vec_base w; + /* Within [-1..1] range, avoid aligned axis. */ + if (LIKELY(math::abs(cosom) < (T(1) - eps))) { + const T omega = math::acos(cosom); + const T sinom = math::sin(omega); + + w[0] = math::sin((T(1) - t) * omega) / sinom; + w[1] = math::sin(t * omega) / sinom; + } + else { + /* Fallback to lerp */ + w[0] = T(1) - t; + w[1] = t; + } + return w; +} + +template +inline detail::Quaternion interpolate(const detail::Quaternion &a, + const detail::Quaternion &b, + T t) +{ + using Vec4T = vec_base; + BLI_assert(is_unit_scale(Vec4T(a))); + BLI_assert(is_unit_scale(Vec4T(b))); + + Vec4T quat = Vec4T(a); + T cosom = dot(Vec4T(a), Vec4T(b)); + /* Rotate around shortest angle. */ + if (cosom < T(0)) { + cosom = -cosom; + quat = -quat; + } + + vec_base w = interpolate_dot_slerp(t, cosom); + + return detail::Quaternion(w[0] * quat + w[1] * Vec4T(b)); +} + +} // namespace blender::math + +/* -------------------------------------------------------------------- */ +/** \name Template implementations + * \{ */ + +namespace blender::math::detail { + +#ifdef DEBUG +# define BLI_ASSERT_UNIT_QUATERNION(_q) \ + { \ + auto rot_vec = static_cast>(_q); \ + T quat_length = math::length_squared(rot_vec); \ + if (!(quat_length == 0 || (math::abs(quat_length - 1) < 0.0001))) { \ + std::cout << "Warning! " << __func__ << " called with non-normalized quaternion: size " \ + << quat_length << " *** report a bug ***\n"; \ + } \ + } +#else +# define BLI_ASSERT_UNIT_QUATERNION(_q) +#endif + +template AxisAngle::AxisAngle(const vec_base &axis, T angle) +{ + T length; + axis_ = math::normalize_and_get_length(axis, length); + if (length > 0.0f) { + angle_cos_ = math::cos(angle); + angle_sin_ = math::sin(angle); + angle_ = angle; + } + else { + *this = identity(); + } +} + +template AxisAngle::AxisAngle(const vec_base &from, const vec_base &to) +{ + BLI_assert(is_unit_scale(from)); + BLI_assert(is_unit_scale(to)); + + /* Avoid calculating the angle. */ + angle_cos_ = dot(from, to); + axis_ = normalize_and_get_length(cross(from, to), angle_sin_); + + if (angle_sin_ <= FLT_EPSILON) { + if (angle_cos_ > T(0)) { + /* Same vectors, zero rotation... */ + *this = identity(); + } + else { + /* Colinear but opposed vectors, 180 rotation... */ + axis_ = normalize(orthogonal(from)); + angle_sin_ = T(0); + angle_cos_ = T(-1); + } + } +} +template +AxisAngleNormalized::AxisAngleNormalized(const vec_base &axis, T angle) : AxisAngle() +{ + BLI_assert(is_unit_scale(axis)); + this->axis_ = axis; + this->angle_ = angle; + this->angle_cos_ = math::cos(angle); + this->angle_sin_ = math::sin(angle); +} + +template Quaternion::operator EulerXYZ() const +{ + using Mat3T = MatBase; + const Quaternion &quat = *this; + BLI_ASSERT_UNIT_QUATERNION(quat) + Mat3T unit_mat = math::from_rotation(quat); + return math::to_euler(unit_mat); +} + +template EulerXYZ::operator Quaternion() const +{ + const EulerXYZ &eul = *this; + const T ti = eul.x * T(0.5); + const T tj = eul.y * T(0.5); + const T th = eul.z * T(0.5); + const T ci = math::cos(ti); + const T cj = math::cos(tj); + const T ch = math::cos(th); + const T si = math::sin(ti); + const T sj = math::sin(tj); + const T sh = math::sin(th); + const T cc = ci * ch; + const T cs = ci * sh; + const T sc = si * ch; + const T ss = si * sh; + + Quaternion quat; + quat.x = cj * cc + sj * ss; + quat.y = cj * sc - sj * cs; + quat.z = cj * ss + sj * cc; + quat.w = cj * cs - sj * sc; + return quat; +} + +template Quaternion::operator AxisAngle() const +{ + const Quaternion &quat = *this; + BLI_ASSERT_UNIT_QUATERNION(quat) + + /* Calculate angle/2, and sin(angle/2). */ + T ha = math::acos(quat.x); + T si = math::sin(ha); + + /* From half-angle to angle. */ + T angle = ha * 2; + /* Prevent division by zero for axis conversion. */ + if (math::abs(si) < 0.0005) { + si = 1.0f; + } + + vec_base axis = vec_base(quat.y, quat.z, quat.w) / si; + if (math::is_zero(axis)) { + axis[1] = 1.0f; + } + return AxisAngleNormalized(axis, angle); +} + +template AxisAngle::operator Quaternion() const +{ + BLI_assert(math::is_unit_scale(axis_)); + + /** Using half angle identities: sin(angle / 2) = sqrt((1 - angle_cos) / 2) */ + T sine = math::sqrt(T(0.5) - angle_cos_ * T(0.5)); + const T cosine = math::sqrt(T(0.5) + angle_cos_ * T(0.5)); + + if (angle_sin_ < 0.0) { + sine = -sine; + } + + Quaternion quat; + quat.x = cosine; + quat.y = axis_.x * sine; + quat.z = axis_.y * sine; + quat.w = axis_.z * sine; + return quat; +} + +template EulerXYZ::operator AxisAngle() const +{ + /* Use quaternions as intermediate representation for now... */ + return AxisAngle(Quaternion(*this)); +} + +template AxisAngle::operator EulerXYZ() const +{ + /* Use quaternions as intermediate representation for now... */ + return EulerXYZ(Quaternion(*this)); +} + +/* Using explicit template instantiations in order to reduce compilation time. */ +extern template AxisAngle::operator EulerXYZ() const; +extern template AxisAngle::operator Quaternion() const; +extern template EulerXYZ::operator AxisAngle() const; +extern template EulerXYZ::operator Quaternion() const; +extern template Quaternion::operator AxisAngle() const; +extern template Quaternion::operator EulerXYZ() const; + +extern template AxisAngle::operator EulerXYZ() const; +extern template AxisAngle::operator Quaternion() const; +extern template EulerXYZ::operator AxisAngle() const; +extern template EulerXYZ::operator Quaternion() const; +extern template Quaternion::operator AxisAngle() const; +extern template Quaternion::operator EulerXYZ() const; + +} // namespace blender::math::detail + +/** \} */ diff --git a/source/blender/blenlib/BLI_math_rotation_types.hh b/source/blender/blenlib/BLI_math_rotation_types.hh new file mode 100644 index 00000000000..1689789db4b --- /dev/null +++ b/source/blender/blenlib/BLI_math_rotation_types.hh @@ -0,0 +1,244 @@ +/* SPDX-License-Identifier: GPL-2.0-or-later */ + +#pragma once + +/** \file + * \ingroup bli + */ + +#include "BLI_math_base.hh" +#include "BLI_math_vec_types.hh" + +namespace blender::math { + +namespace detail { + +/** + * Rotation Types + * + * It gives more semantic informations allowing overloaded functions based on the rotation + * type. It also prevent implicit cast from rotation to vector types. + */ + +/* Forward declaration. */ +template struct AxisAngle; +template struct Quaternion; + +template struct EulerXYZ { + T x, y, z; + + EulerXYZ() = default; + + EulerXYZ(const T &x, const T &y, const T &z) + { + this->x = x; + this->y = y; + this->z = z; + } + + EulerXYZ(const vec_base &vec) : EulerXYZ(UNPACK3(vec)){}; + + /** Static functions. */ + + static EulerXYZ identity() + { + return {0, 0, 0}; + } + + /** Conversions. */ + + explicit operator vec_base() const + { + return {this->x, this->y, this->z}; + } + + explicit operator AxisAngle() const; + + explicit operator Quaternion() const; + + /** Operators. */ + + friend std::ostream &operator<<(std::ostream &stream, const EulerXYZ &rot) + { + return stream << "EulerXYZ" << static_cast>(rot); + } +}; + +template struct Quaternion { + T x, y, z, w; + + Quaternion() = default; + + Quaternion(const T &x, const T &y, const T &z, const T &w) + { + this->x = x; + this->y = y; + this->z = z; + this->w = w; + } + + Quaternion(const vec_base &vec) : Quaternion(UNPACK4(vec)){}; + + /** Static functions. */ + + static Quaternion identity() + { + return {1, 0, 0, 0}; + } + + /** Conversions. */ + + explicit operator vec_base() const + { + return {this->x, this->y, this->z, this->w}; + } + + explicit operator EulerXYZ() const; + + explicit operator AxisAngle() const; + + /** Operators. */ + + const T &operator[](int i) const + { + BLI_assert(i >= 0 && i < 4); + return (&this->x)[i]; + } + + friend std::ostream &operator<<(std::ostream &stream, const Quaternion &rot) + { + return stream << "Quaternion" << static_cast>(rot); + } +}; + +template struct AxisAngle { + using vec3_type = vec_base; + + protected: + vec3_type axis_ = {0, 1, 0}; + /** Store cosine and sine so rotation is cheaper and doesn't require atan2. */ + T angle_cos_ = 1; + T angle_sin_ = 0; + /** + * Source angle for interpolation. + * It might not be computed on creation, so the getter ensures it is updated. + */ + T angle_ = 0; + + /** + * A defaulted constructor would cause zero initialization instead of default initialization, + * and not call the default member initializers. + */ + explicit AxisAngle(){}; + + public: + /** + * Create a rotation from an axis and an angle. + * \note `axis` does not have to be normalized. + * Use `AxisAngleNormalized` instead to skip normalization cost. + */ + AxisAngle(const vec3_type &axis, T angle); + + /** + * Create a rotation from 2 normalized vectors. + * \note `from` and `to` must be normalized. + */ + AxisAngle(const vec3_type &from, const vec3_type &to); + + /** Static functions. */ + + static AxisAngle identity() + { + return AxisAngle(); + } + + /** Getters. */ + + const vec3_type &axis() const + { + return axis_; + } + + const T &angle() const + { + if (UNLIKELY(angle_ == T(0) && angle_cos_ != T(1))) { + /* Angle wasn't computed by constructor. */ + const_cast(this)->angle_ = math::atan2(angle_sin_, angle_cos_); + } + return angle_; + } + + const T &angle_cos() const + { + return angle_cos_; + } + + const T &angle_sin() const + { + return angle_sin_; + } + + /** Conversions. */ + + explicit operator Quaternion() const; + + explicit operator EulerXYZ() const; + + /** Operators. */ + + friend bool operator==(const AxisAngle &a, const AxisAngle &b) + { + return (a.axis == b.axis) && (a.angle == b.angle); + } + + friend bool operator!=(const AxisAngle &a, const AxisAngle &b) + { + return (a != b); + } + + friend std::ostream &operator<<(std::ostream &stream, const AxisAngle &rot) + { + return stream << "AxisAngle(axis=" << rot.axis << ", angle=" << rot.angle << ")"; + } +}; + +/** + * A version of AxisAngle that expects axis to be already normalized. + * Implicitly cast back to AxisAngle. + */ +template struct AxisAngleNormalized : public AxisAngle { + AxisAngleNormalized(const vec_base &axis, T angle); + + operator AxisAngle() const + { + return *this; + } +}; + +/** + * Intermediate Types. + * + * Some functions need to have higher precision than standard floats for some operations. + */ +template struct TypeTraits { + using DoublePrecision = T; +}; +template<> struct TypeTraits { + using DoublePrecision = double; +}; + +}; // namespace detail + +template struct AssertUnitEpsilon> { + static constexpr U value = AssertUnitEpsilon::value * 10; +}; + +/* Most common used types. */ +using EulerXYZ = math::detail::EulerXYZ; +using Quaternion = math::detail::Quaternion; +using AxisAngle = math::detail::AxisAngle; +using AxisAngleNormalized = math::detail::AxisAngleNormalized; + +} // namespace blender::math + +/** \} */ diff --git a/source/blender/blenlib/BLI_math_vec_types.hh b/source/blender/blenlib/BLI_math_vec_types.hh index f57b4b44e61..0b3bb46db07 100644 --- a/source/blender/blenlib/BLI_math_vec_types.hh +++ b/source/blender/blenlib/BLI_math_vec_types.hh @@ -602,6 +602,15 @@ template struct vec_base : public vec_struct_base } }; +namespace math { + +template struct AssertUnitEpsilon { + /** \note Copy of BLI_ASSERT_UNIT_EPSILON_DB to avoid dragging the entire header. */ + static constexpr T value = T(0.0002); +}; + +} // namespace math + using char3 = blender::vec_base; using uchar3 = blender::vec_base; diff --git a/source/blender/blenlib/BLI_math_vector.hh b/source/blender/blenlib/BLI_math_vector.hh index 6ce033ea376..632be6316ad 100644 --- a/source/blender/blenlib/BLI_math_vector.hh +++ b/source/blender/blenlib/BLI_math_vector.hh @@ -17,18 +17,6 @@ namespace blender::math { -#ifndef NDEBUG -# define BLI_ASSERT_UNIT(v) \ - { \ - const float _test_unit = length_squared(v); \ - BLI_assert(!(std::abs(_test_unit - 1.0f) >= BLI_ASSERT_UNIT_EPSILON) || \ - !(std::abs(_test_unit) >= BLI_ASSERT_UNIT_EPSILON)); \ - } \ - (void)0 -#else -# define BLI_ASSERT_UNIT(v) (void)(v) -#endif - template inline bool is_zero(const vec_base &a) { for (int i = 0; i < Size; i++) { @@ -137,7 +125,7 @@ inline vec_base mod(const vec_base &a, const T &b) } template))> -inline T safe_mod(const vec_base &a, const vec_base &b) +inline vec_base safe_mod(const vec_base &a, const vec_base &b) { vec_base result; for (int i = 0; i < Size; i++) { @@ -147,7 +135,7 @@ inline T safe_mod(const vec_base &a, const vec_base &b) } template))> -inline T safe_mod(const vec_base &a, const T &b) +inline vec_base safe_mod(const vec_base &a, const T &b) { if (b == 0) { return vec_base(0); @@ -278,6 +266,16 @@ inline T length(const vec_base &a) return std::sqrt(length_squared(a)); } +template inline bool is_unit_scale(const vec_base &v) +{ + /* Checks are flipped so NAN doesn't assert because we're making sure the value was + * normalized and in the case we don't want NAN to be raising asserts since there + * is nothing to be done in that case. */ + const T test_unit = math::length_squared(v); + return (!(std::abs(test_unit - T(1)) >= AssertUnitEpsilon::value) || + !(std::abs(test_unit) >= AssertUnitEpsilon::value)); +} + template))> inline T distance_manhattan(const vec_base &a, const vec_base &b) { @@ -300,7 +298,7 @@ template))> inline vec_base reflect(const vec_base &incident, const vec_base &normal) { - BLI_ASSERT_UNIT(normal); + BLI_assert(is_unit_scale(normal)); return incident - 2.0 * dot(normal, incident) * normal; } @@ -400,6 +398,9 @@ inline vec_base midpoint(const vec_base &a, const vec_base))> inline vec_base faceforward(const vec_base &vector, const vec_base &incident, diff --git a/source/blender/blenlib/CMakeLists.txt b/source/blender/blenlib/CMakeLists.txt index 17218a2daed..c554c19763b 100644 --- a/source/blender/blenlib/CMakeLists.txt +++ b/source/blender/blenlib/CMakeLists.txt @@ -18,6 +18,7 @@ set(INC ) set(INC_SYS + ${EIGEN3_INCLUDE_DIRS} ${ZLIB_INCLUDE_DIRS} ${ZSTD_INCLUDE_DIRS} ${GMP_INCLUDE_DIRS} @@ -102,6 +103,7 @@ set(SRC intern/math_geom_inline.c intern/math_interp.c intern/math_matrix.c + intern/math_matrix.cc intern/math_rotation.c intern/math_rotation.cc intern/math_solvers.c @@ -485,6 +487,7 @@ if(WITH_GTESTS) tests/BLI_math_bits_test.cc tests/BLI_math_color_test.cc tests/BLI_math_geom_test.cc + tests/BLI_math_matrix_types_test.cc tests/BLI_math_matrix_test.cc tests/BLI_math_rotation_test.cc tests/BLI_math_solvers_test.cc diff --git a/source/blender/blenlib/intern/math_matrix.cc b/source/blender/blenlib/intern/math_matrix.cc new file mode 100644 index 00000000000..9211a608022 --- /dev/null +++ b/source/blender/blenlib/intern/math_matrix.cc @@ -0,0 +1,457 @@ +/* SPDX-License-Identifier: GPL-2.0-or-later + * Copyright 2022 Blender Foundation. */ + +/** \file + * \ingroup bli + */ + +#include "BLI_math_matrix.hh" +#include "BLI_math_rotation.hh" + +#include +#include +#include + +/* -------------------------------------------------------------------- */ +/** \name Matrix multiplication + * \{ */ + +namespace blender { + +template<> float4x4 float4x4::operator*(const float4x4 &b) const +{ + using namespace math; + const float4x4 &a = *this; + float4x4 result; + +#ifdef BLI_HAVE_SSE2 + __m128 A0 = _mm_load_ps(a[0]); + __m128 A1 = _mm_load_ps(a[1]); + __m128 A2 = _mm_load_ps(a[2]); + __m128 A3 = _mm_load_ps(a[3]); + + for (int i = 0; i < 4; i++) { + __m128 B0 = _mm_set1_ps(b[i][0]); + __m128 B1 = _mm_set1_ps(b[i][1]); + __m128 B2 = _mm_set1_ps(b[i][2]); + __m128 B3 = _mm_set1_ps(b[i][3]); + + __m128 sum = _mm_add_ps(_mm_add_ps(_mm_mul_ps(B0, A0), _mm_mul_ps(B1, A1)), + _mm_add_ps(_mm_mul_ps(B2, A2), _mm_mul_ps(B3, A3))); + + _mm_store_ps(result[i], sum); + } +#else + const float4x4 T = transpose(b); + + result.x = float4(dot(a.x, T.x), dot(a.x, T.y), dot(a.x, T.z), dot(a.x, T.w)); + result.y = float4(dot(a.y, T.x), dot(a.y, T.y), dot(a.y, T.z), dot(a.y, T.w)); + result.z = float4(dot(a.z, T.x), dot(a.z, T.y), dot(a.z, T.z), dot(a.z, T.w)); + result.w = float4(dot(a.w, T.x), dot(a.w, T.y), dot(a.w, T.z), dot(a.w, T.w)); +#endif + return result; +} + +template<> float3x3 float3x3::operator*(const float3x3 &b) const +{ + using namespace math; + const float3x3 &a = *this; + float3x3 result; + +#if 0 /* 1.2 times slower. Could be used as reference for aligned version. */ + __m128 A0 = _mm_set_ps(0, a[0][2], a[0][1], a[0][0]); + __m128 A1 = _mm_set_ps(0, a[1][2], a[1][1], a[1][0]); + __m128 A2 = _mm_set_ps(0, a[2][2], a[2][1], a[2][0]); + + for (int i = 0; i < 2; i++) { + __m128 B0 = _mm_set1_ps(b[i][0]); + __m128 B1 = _mm_set1_ps(b[i][1]); + __m128 B2 = _mm_set1_ps(b[i][2]); + __m128 sum = _mm_add_ps(_mm_add_ps(_mm_mul_ps(B0, A0), _mm_mul_ps(B1, A1)), + _mm_mul_ps(B2, A2)); + _mm_storeu_ps(result[i], sum); + } + + _mm_storeu_ps(result[1], sum[1]); + /* Manual per component store to avoid segfault. */ + _mm_store_ss(&result[2][0], sum[2]); + sum[2] = _mm_shuffle_ps(sum[2], sum[2], _MM_SHUFFLE(3, 2, 1, 1)); + _mm_store_ss(&result[2][1], sum[2]); + sum[2] = _mm_shuffle_ps(sum[2], sum[2], _MM_SHUFFLE(3, 2, 1, 2)); + _mm_store_ss(&result[2][2], sum[2]); + +#else + /** Manual unrolling since MSVC doesn't seem to unroll properly. */ + result[0][0] = b[0][0] * a[0][0] + b[0][1] * a[1][0] + b[0][2] * a[2][0]; + result[0][1] = b[0][0] * a[0][1] + b[0][1] * a[1][1] + b[0][2] * a[2][1]; + result[0][2] = b[0][0] * a[0][2] + b[0][1] * a[1][2] + b[0][2] * a[2][2]; + + result[1][0] = b[1][0] * a[0][0] + b[1][1] * a[1][0] + b[1][2] * a[2][0]; + result[1][1] = b[1][0] * a[0][1] + b[1][1] * a[1][1] + b[1][2] * a[2][1]; + result[1][2] = b[1][0] * a[0][2] + b[1][1] * a[1][2] + b[1][2] * a[2][2]; + + result[2][0] = b[2][0] * a[0][0] + b[2][1] * a[1][0] + b[2][2] * a[2][0]; + result[2][1] = b[2][0] * a[0][1] + b[2][1] * a[1][1] + b[2][2] * a[2][1]; + result[2][2] = b[2][0] * a[0][2] + b[2][1] * a[1][2] + b[2][2] * a[2][2]; +#endif + return result; +} + +template float2x2 float2x2::operator*(const float2x2 &b) const; +template double2x2 double2x2::operator*(const double2x2 &b) const; +template double3x3 double3x3::operator*(const double3x3 &b) const; +template double4x4 double4x4::operator*(const double4x4 &b) const; + +} // namespace blender + +/** \} */ + +namespace blender::math { + +/* -------------------------------------------------------------------- */ +/** \name Determinant + * \{ */ + +template T determinant(const MatBase &mat) +{ + return Eigen::Map>(mat.base_ptr()).determinant(); +} + +template float determinant(const float2x2 &mat); +template float determinant(const float3x3 &mat); +template float determinant(const float4x4 &mat); +template double determinant(const double2x2 &mat); +template double determinant(const double3x3 &mat); +template double determinant(const double4x4 &mat); + +template bool is_negative(const MatBase &mat) +{ + return Eigen::Map, 0, Eigen::Stride<4, 1>>(mat.base_ptr()) + .determinant() < T(0); +} + +template bool is_negative(const float4x4 &mat); +template bool is_negative(const double4x4 &mat); + +/** \} */ + +/* -------------------------------------------------------------------- */ +/** \name Adjoint + * \{ */ + +template MatBase adjoint(const MatBase &mat) +{ + MatBase adj; + unroll([&](auto c) { + unroll([&](auto r) { + /* Copy other cells except the "cross" to compute the determinant. */ + MatBase tmp; + unroll([&](auto m_c) { + unroll([&](auto m_r) { + if (m_c != c && m_r != r) { + int d_c = (m_c < c) ? m_c : (m_c - 1); + int d_r = (m_r < r) ? m_r : (m_r - 1); + tmp[d_c][d_r] = mat[m_c][m_r]; + } + }); + }); + T minor = determinant(tmp); + /* Transpose directly to get the adjugate. Swap destination row and col. */ + adj[r][c] = ((c + r) & 1) ? -minor : minor; + }); + }); + return adj; +} + +template float2x2 adjoint(const float2x2 &mat); +template float3x3 adjoint(const float3x3 &mat); +template float4x4 adjoint(const float4x4 &mat); +template double2x2 adjoint(const double2x2 &mat); +template double3x3 adjoint(const double3x3 &mat); +template double4x4 adjoint(const double4x4 &mat); + +/** \} */ + +/* -------------------------------------------------------------------- */ +/** \name Inverse + * \{ */ + +template +MatBase invert(const MatBase &mat, bool &r_success) +{ + MatBase result; + Eigen::Map> M(mat.base_ptr()); + Eigen::Map> R(result.base_ptr()); + M.computeInverseWithCheck(R, r_success, 0.0f); + if (!r_success) { + R = R.Zero(); + } + return result; +} + +template float2x2 invert(const float2x2 &mat, bool &r_success); +template float3x3 invert(const float3x3 &mat, bool &r_success); +template float4x4 invert(const float4x4 &mat, bool &r_success); +template double2x2 invert(const double2x2 &mat, bool &r_success); +template double3x3 invert(const double3x3 &mat, bool &r_success); +template double4x4 invert(const double4x4 &mat, bool &r_success); + +template +MatBase pseudo_invert(const MatBase &mat, T epsilon) +{ + /* Start by trying normal inversion first. */ + bool success; + MatBase inv = invert(mat, success); + if (success) { + return inv; + } + + /** + * Compute the Single Value Decomposition of an arbitrary matrix A + * That is compute the 3 matrices U,W,V with U column orthogonal (m,n) + * ,W a diagonal matrix and V an orthogonal square matrix `s.t.A = U.W.Vt`. + * From this decomposition it is trivial to compute the (pseudo-inverse) + * of `A` as `Ainv = V.Winv.transpose(U)`. + */ + MatBase U, W, V; + vec_base S_val; + + { + using namespace Eigen; + using MatrixT = Eigen::Matrix; + using VectorT = Eigen::Matrix; + /* Blender and Eigen matrices are both column-major by default. + * Since our matrix is squared, we can use thinU/V. */ + /** WORKAROUND: + * (ComputeThinU | ComputeThinV) must be set as runtime parameters in Eigen < 3.4.0. + * But this requires the matrix type to be dynamic to avoid an assert. + */ + using MatrixDynamicT = Eigen::Matrix; + JacobiSVD svd( + Eigen::Map(mat.base_ptr(), Size, Size), ComputeThinU | ComputeThinV); + + (Eigen::Map(U.base_ptr())) = svd.matrixU(); + (Eigen::Map(S_val)) = svd.singularValues(); + (Eigen::Map(V.base_ptr())) = svd.matrixV(); + } + + /* Invert or nullify component based on epsilon comparison. */ + unroll([&](auto i) { S_val[i] = (S_val[i] < epsilon) ? T(0) : (T(1) / S_val[i]); }); + + W = from_scale>(S_val); + return (V * W) * transpose(U); +} + +template float2x2 pseudo_invert(const float2x2 &mat, float epsilon); +template float3x3 pseudo_invert(const float3x3 &mat, float epsilon); +template float4x4 pseudo_invert(const float4x4 &mat, float epsilon); +template double2x2 pseudo_invert(const double2x2 &mat, double epsilon); +template double3x3 pseudo_invert(const double3x3 &mat, double epsilon); +template double4x4 pseudo_invert(const double4x4 &mat, double epsilon); + +/** \} */ + +/* -------------------------------------------------------------------- */ +/** \name Polar Decomposition + * \{ */ + +/** + * Right polar decomposition: + * M = UP + * + * U is the 'rotation'-like component, the closest orthogonal matrix to M. + * P is the 'scaling'-like component, defined in U space. + * + * See https://en.wikipedia.org/wiki/Polar_decomposition for more. + */ +template +static void polar_decompose(const MatBase &mat3, + MatBase &r_U, + MatBase &r_P) +{ + /* From svd decomposition (M = WSV*), we have: + * U = WV* + * P = VSV* + */ + MatBase W, V; + vec_base S_val; + + { + using namespace Eigen; + using MatrixT = Eigen::Matrix; + using VectorT = Eigen::Matrix; + /* Blender and Eigen matrices are both column-major by default. + * Since our matrix is squared, we can use thinU/V. */ + /** WORKAROUND: + * (ComputeThinU | ComputeThinV) must be set as runtime parameters in Eigen < 3.4.0. + * But this requires the matrix type to be dynamic to avoid an assert. + */ + using MatrixDynamicT = Eigen::Matrix; + JacobiSVD svd( + Eigen::Map(mat3.base_ptr(), 3, 3), ComputeThinU | ComputeThinV); + + (Eigen::Map(W.base_ptr())) = svd.matrixU(); + (Eigen::Map(S_val)) = svd.singularValues(); + (Map(V.base_ptr())) = svd.matrixV(); + } + + MatBase S = from_scale>(S_val); + MatBase Vt = transpose(V); + + r_U = W * Vt; + r_P = (V * S) * Vt; +} + +/** \} */ + +/* -------------------------------------------------------------------- */ +/** \name Interpolate + * \{ */ + +template +MatBase interpolate(const MatBase &A, const MatBase &B, T t) +{ + using Mat3T = MatBase; + /* 'Rotation' component ('U' part of polar decomposition, + * the closest orthogonal matrix to M3 rot/scale + * transformation matrix), spherically interpolated. */ + Mat3T U_A, U_B; + /* 'Scaling' component ('P' part of polar decomposition, i.e. scaling in U-defined space), + * linearly interpolated. */ + Mat3T P_A, P_B; + + polar_decompose(A, U_A, P_A); + polar_decompose(B, U_B, P_B); + + /* Quaternions cannot represent an axis flip. If such a singularity is detected, choose a + * different decomposition of the matrix that still satisfies A = U_A * P_A but which has a + * positive determinant and thus no axis flips. This resolves T77154. + * + * Note that a flip of two axes is just a rotation of 180 degrees around the third axis, and + * three flipped axes are just an 180 degree rotation + a single axis flip. It is thus sufficient + * to solve this problem for single axis flips. */ + if (is_negative(U_A)) { + U_A = -U_A; + P_A = -P_A; + } + if (is_negative(U_B)) { + U_B = -U_B; + P_B = -P_B; + } + + detail::Quaternion quat_A = math::to_quaternion(U_A); + detail::Quaternion quat_B = math::to_quaternion(U_B); + detail::Quaternion quat = math::interpolate(quat_A, quat_B, t); + Mat3T U = from_rotation(quat); + + Mat3T P = interpolate_linear(P_A, P_B, t); + /* And we reconstruct rot/scale matrix from interpolated polar components */ + return U * P; +} + +template float3x3 interpolate(const float3x3 &a, const float3x3 &b, float t); +template double3x3 interpolate(const double3x3 &a, const double3x3 &b, double t); + +template +MatBase interpolate(const MatBase &A, const MatBase &B, T t) +{ + MatBase result = MatBase( + interpolate(MatBase(A), MatBase(B), t)); + + /* Location component, linearly interpolated. */ + const auto &loc_a = static_cast &>(A).location(); + const auto &loc_b = static_cast &>(B).location(); + result.location() = interpolate(loc_a, loc_b, t); + + return result; +} + +template float4x4 interpolate(const float4x4 &a, const float4x4 &b, float t); +template double4x4 interpolate(const double4x4 &a, const double4x4 &b, double t); + +template +MatBase interpolate_fast(const MatBase &a, const MatBase &b, T t) +{ + using QuaternionT = detail::Quaternion; + using Vec3T = typename MatBase::vec3_type; + + Vec3T a_scale, b_scale; + QuaternionT a_quat, b_quat; + to_rot_scale(a, a_quat, a_scale); + to_rot_scale(b, b_quat, b_scale); + + const Vec3T scale = interpolate(a_scale, b_scale, t); + const QuaternionT rotation = interpolate(a_quat, b_quat, t); + return from_rot_scale>(rotation, scale); +} + +template float3x3 interpolate_fast(const float3x3 &a, const float3x3 &b, float t); +template double3x3 interpolate_fast(const double3x3 &a, const double3x3 &b, double t); + +template +MatBase interpolate_fast(const MatBase &a, const MatBase &b, T t) +{ + using QuaternionT = detail::Quaternion; + using Vec3T = typename MatBase::vec3_type; + + Vec3T a_loc, b_loc; + Vec3T a_scale, b_scale; + QuaternionT a_quat, b_quat; + to_loc_rot_scale(a, a_loc, a_quat, a_scale); + to_loc_rot_scale(b, b_loc, b_quat, b_scale); + + const Vec3T location = interpolate(a_loc, b_loc, t); + const Vec3T scale = interpolate(a_scale, b_scale, t); + const QuaternionT rotation = interpolate(a_quat, b_quat, t); + return from_loc_rot_scale>(location, rotation, scale); +} + +template float4x4 interpolate_fast(const float4x4 &a, const float4x4 &b, float t); +template double4x4 interpolate_fast(const double4x4 &a, const double4x4 &b, double t); + +/** \} */ + +/* -------------------------------------------------------------------- */ +/** \name Template instantiation + * \{ */ + +namespace detail { + +template void normalized_to_eul2(const float3x3 &mat, + detail::EulerXYZ &eul1, + detail::EulerXYZ &eul2); +template void normalized_to_eul2(const double3x3 &mat, + detail::EulerXYZ &eul1, + detail::EulerXYZ &eul2); + +template detail::Quaternion normalized_to_quat_with_checks(const float3x3 &mat); +template detail::Quaternion normalized_to_quat_with_checks(const double3x3 &mat); + +template MatBase from_rotation(const detail::EulerXYZ &rotation); +template MatBase from_rotation(const detail::EulerXYZ &rotation); +template MatBase from_rotation(const detail::Quaternion &rotation); +template MatBase from_rotation(const detail::Quaternion &rotation); +template MatBase from_rotation(const detail::AxisAngle &rotation); +template MatBase from_rotation(const detail::AxisAngle &rotation); + +} // namespace detail + +template float3 transform_point(const float3x3 &mat, const float3 &point); +template float3 transform_point(const float4x4 &mat, const float3 &point); +template float3 transform_direction(const float3x3 &mat, const float3 &direction); +template float3 transform_direction(const float4x4 &mat, const float3 &direction); +template float3 project_point(const float4x4 &mat, const float3 &point); +template float2 project_point(const float3x3 &mat, const float2 &point); + +namespace projection { + +template float4x4 orthographic( + float left, float right, float bottom, float top, float near_clip, float far_clip); +template float4x4 perspective( + float left, float right, float bottom, float top, float near_clip, float far_clip); + +} // namespace projection + +/** \} */ + +} // namespace blender::math diff --git a/source/blender/blenlib/intern/math_rotation.cc b/source/blender/blenlib/intern/math_rotation.cc index 454862c8d18..968d5bc57c0 100644 --- a/source/blender/blenlib/intern/math_rotation.cc +++ b/source/blender/blenlib/intern/math_rotation.cc @@ -5,10 +5,30 @@ */ #include "BLI_math_base.h" +#include "BLI_math_matrix.hh" +#include "BLI_math_rotation.hh" #include "BLI_math_rotation_legacy.hh" #include "BLI_math_vector.h" #include "BLI_math_vector.hh" +namespace blender::math::detail { + +template AxisAngle::operator EulerXYZ() const; +template AxisAngle::operator Quaternion() const; +template EulerXYZ::operator AxisAngle() const; +template EulerXYZ::operator Quaternion() const; +template Quaternion::operator AxisAngle() const; +template Quaternion::operator EulerXYZ() const; + +template AxisAngle::operator EulerXYZ() const; +template AxisAngle::operator Quaternion() const; +template EulerXYZ::operator AxisAngle() const; +template EulerXYZ::operator Quaternion() const; +template Quaternion::operator AxisAngle() const; +template Quaternion::operator EulerXYZ() const; + +} // namespace blender::math::detail + namespace blender::math { float3 rotate_direction_around_axis(const float3 &direction, const float3 &axis, const float angle) diff --git a/source/blender/blenlib/tests/BLI_math_matrix_test.cc b/source/blender/blenlib/tests/BLI_math_matrix_test.cc index e429be231b4..71c99a00807 100644 --- a/source/blender/blenlib/tests/BLI_math_matrix_test.cc +++ b/source/blender/blenlib/tests/BLI_math_matrix_test.cc @@ -3,6 +3,8 @@ #include "testing/testing.h" #include "BLI_math_matrix.h" +#include "BLI_math_matrix.hh" +#include "BLI_math_rotation.hh" TEST(math_matrix, interp_m4_m4m4_regular) { @@ -62,7 +64,7 @@ TEST(math_matrix, interp_m3_m3m3_singularity) transpose_m3(matrix_a); EXPECT_NEAR(-1.0f, determinant_m3_array(matrix_a), 1e-6); - /* This matrix represents R=(0, 0, 0), S=(-1, 0, 0) */ + /* This matrix represents R=(0, 0, 0), S=(-1, 1, 1) */ float matrix_b[3][3] = { {-1.0f, 0.0f, 0.0f}, {0.0f, 1.0f, 0.0f}, @@ -97,3 +99,372 @@ TEST(math_matrix, interp_m3_m3m3_singularity) EXPECT_NEAR(0.0f, determinant_m3_array(result), 1e-5); EXPECT_M3_NEAR(result, expect, 1e-5); } + +namespace blender::tests { + +using namespace blender::math; + +TEST(math_matrix, MatrixInverse) +{ + float3x3 mat = float3x3::diagonal(2); + float3x3 inv = invert(mat); + float3x3 expect = float3x3({0.5f, 0.0f, 0.0f}, {0.0f, 0.5f, 0.0f}, {0.0f, 0.0f, 0.5f}); + EXPECT_M3_NEAR(inv, expect, 1e-5f); + + bool success; + float3x3 mat2 = float3x3::all(1); + float3x3 inv2 = invert(mat2, success); + float3x3 expect2 = float3x3::all(0); + EXPECT_M3_NEAR(inv2, expect2, 1e-5f); + EXPECT_FALSE(success); +} + +TEST(math_matrix, MatrixPseudoInverse) +{ + float4x4 mat = transpose(float4x4({0.224976f, -0.333770f, 0.765074f, 0.100000f}, + {0.389669f, 0.647565f, 0.168130f, 0.200000f}, + {-0.536231f, 0.330541f, 0.443163f, 0.300000f}, + {0.000000f, 0.000000f, 0.000000f, 1.000000f})); + float4x4 expect = transpose(float4x4({0.224976f, -0.333770f, 0.765074f, 0.100000f}, + {0.389669f, 0.647565f, 0.168130f, 0.200000f}, + {-0.536231f, 0.330541f, 0.443163f, 0.300000f}, + {0.000000f, 0.000000f, 0.000000f, 1.000000f})); + float4x4 inv = pseudo_invert(mat); + pseudoinverse_m4_m4(expect.ptr(), mat.ptr(), 1e-8f); + EXPECT_M4_NEAR(inv, expect, 1e-5f); + + float4x4 mat2 = transpose(float4x4({0.000000f, -0.333770f, 0.765074f, 0.100000f}, + {0.000000f, 0.647565f, 0.168130f, 0.200000f}, + {0.000000f, 0.330541f, 0.443163f, 0.300000f}, + {0.000000f, 0.000000f, 0.000000f, 1.000000f})); + float4x4 expect2 = transpose(float4x4({0.000000f, 0.000000f, 0.000000f, 0.000000f}, + {-0.51311f, 1.02638f, 0.496437f, -0.302896f}, + {0.952803f, 0.221885f, 0.527413f, -0.297881f}, + {-0.0275438f, -0.0477073f, 0.0656508f, 0.9926f})); + float4x4 inv2 = pseudo_invert(mat2); + EXPECT_M4_NEAR(inv2, expect2, 1e-5f); +} + +TEST(math_matrix, MatrixDeterminant) +{ + float2x2 m2({1, 2}, {3, 4}); + float3x3 m3({1, 2, 3}, {-3, 4, -5}, {5, -6, 7}); + float4x4 m4({1, 2, -3, 3}, {3, 4, -5, 3}, {5, 6, 7, -3}, {5, 6, 7, 1}); + EXPECT_NEAR(determinant(m2), -2.0f, 1e-8f); + EXPECT_NEAR(determinant(m3), -16.0f, 1e-8f); + EXPECT_NEAR(determinant(m4), -112.0f, 1e-8f); + EXPECT_NEAR(determinant(double2x2(m2)), -2.0f, 1e-8f); + EXPECT_NEAR(determinant(double3x3(m3)), -16.0f, 1e-8f); + EXPECT_NEAR(determinant(double4x4(m4)), -112.0f, 1e-8f); +} + +TEST(math_matrix, MatrixAdjoint) +{ + float2x2 m2({1, 2}, {3, 4}); + float3x3 m3({1, 2, 3}, {-3, 4, -5}, {5, -6, 7}); + float4x4 m4({1, 2, -3, 3}, {3, 4, -5, 3}, {5, 6, 7, -3}, {5, 6, 7, 1}); + float2x2 expect2 = transpose(float2x2({4, -3}, {-2, 1})); + float3x3 expect3 = transpose(float3x3({-2, -4, -2}, {-32, -8, 16}, {-22, -4, 10})); + float4x4 expect4 = transpose( + float4x4({232, -184, -8, -0}, {-128, 88, 16, 0}, {80, -76, 4, 28}, {-72, 60, -12, -28})); + EXPECT_M2_NEAR(adjoint(m2), expect2, 1e-8f); + EXPECT_M3_NEAR(adjoint(m3), expect3, 1e-8f); + EXPECT_M4_NEAR(adjoint(m4), expect4, 1e-8f); +} + +TEST(math_matrix, MatrixAccess) +{ + float4x4 m({1, 2, 3, 4}, {5, 6, 7, 8}, {9, 1, 2, 3}, {4, 5, 6, 7}); + /** Access helpers. */ + EXPECT_EQ(m.x_axis(), float3(1, 2, 3)); + EXPECT_EQ(m.y_axis(), float3(5, 6, 7)); + EXPECT_EQ(m.z_axis(), float3(9, 1, 2)); + EXPECT_EQ(m.location(), float3(4, 5, 6)); +} + +TEST(math_matrix, MatrixInit) +{ + float4x4 expect; + + float4x4 m = from_location({1, 2, 3}); + expect = float4x4({1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 1, 0}, {1, 2, 3, 1}); + EXPECT_TRUE(is_equal(m, expect, 0.00001f)); + + expect = transpose(float4x4({0.411982, -0.833738, -0.36763, 0}, + {-0.0587266, -0.426918, 0.902382, 0}, + {-0.909297, -0.350175, -0.224845, 0}, + {0, 0, 0, 1})); + EulerXYZ euler(1, 2, 3); + Quaternion quat(euler); + AxisAngle axis_angle(euler); + m = from_rotation(euler); + EXPECT_M3_NEAR(m, expect, 1e-5); + m = from_rotation(quat); + EXPECT_M3_NEAR(m, expect, 1e-5); + m = from_rotation(axis_angle); + EXPECT_M3_NEAR(m, expect, 1e-5); + + m = from_scale(float4(1, 2, 3, 4)); + expect = float4x4({1, 0, 0, 0}, {0, 2, 0, 0}, {0, 0, 3, 0}, {0, 0, 0, 4}); + EXPECT_TRUE(is_equal(m, expect, 0.00001f)); + + m = from_scale(float3(1, 2, 3)); + expect = float4x4({1, 0, 0, 0}, {0, 2, 0, 0}, {0, 0, 3, 0}, {0, 0, 0, 1}); + EXPECT_TRUE(is_equal(m, expect, 0.00001f)); + + m = from_scale(float2(1, 2)); + expect = float4x4({1, 0, 0, 0}, {0, 2, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}); + EXPECT_TRUE(is_equal(m, expect, 0.00001f)); + + m = from_loc_rot({1, 2, 3}, EulerXYZ{1, 2, 3}); + expect = float4x4({0.411982, -0.0587266, -0.909297, 0}, + {-0.833738, -0.426918, -0.350175, 0}, + {-0.36763, 0.902382, -0.224845, 0}, + {1, 2, 3, 1}); + EXPECT_TRUE(is_equal(m, expect, 0.00001f)); + + m = from_loc_rot_scale({1, 2, 3}, EulerXYZ{1, 2, 3}, float3{1, 2, 3}); + expect = float4x4({0.411982, -0.0587266, -0.909297, 0}, + {-1.66748, -0.853835, -0.700351, 0}, + {-1.10289, 2.70714, -0.674535, 0}, + {1, 2, 3, 1}); + EXPECT_TRUE(is_equal(m, expect, 0.00001f)); +} + +TEST(math_matrix, MatrixModify) +{ + const float epsilon = 1e-6; + float4x4 result, expect; + float4x4 m1 = float4x4({0, 3, 0, 0}, {2, 0, 0, 0}, {0, 0, 2, 0}, {0, 0, 0, 1}); + + expect = float4x4({0, 3, 0, 0}, {2, 0, 0, 0}, {0, 0, 2, 0}, {4, 9, 2, 1}); + result = translate(m1, float3(3, 2, 1)); + EXPECT_M4_NEAR(result, expect, epsilon); + + expect = float4x4({0, 3, 0, 0}, {2, 0, 0, 0}, {0, 0, 2, 0}, {4, 0, 0, 1}); + result = translate(m1, float2(0, 2)); + EXPECT_M4_NEAR(result, expect, epsilon); + + expect = float4x4({0, 0, -2, 0}, {2, 0, 0, 0}, {0, 3, 0, 0}, {0, 0, 0, 1}); + result = rotate(m1, AxisAngle({0, 1, 0}, M_PI_2)); + EXPECT_M4_NEAR(result, expect, epsilon); + + expect = float4x4({0, 9, 0, 0}, {4, 0, 0, 0}, {0, 0, 8, 0}, {0, 0, 0, 1}); + result = scale(m1, float3(3, 2, 4)); + EXPECT_M4_NEAR(result, expect, epsilon); + + expect = float4x4({0, 9, 0, 0}, {4, 0, 0, 0}, {0, 0, 2, 0}, {0, 0, 0, 1}); + result = scale(m1, float2(3, 2)); + EXPECT_M4_NEAR(result, expect, epsilon); +} + +TEST(math_matrix, MatrixCompareTest) +{ + float4x4 m1 = float4x4({0, 3, 0, 0}, {2, 0, 0, 0}, {0, 0, 2, 0}, {0, 0, 0, 1}); + float4x4 m2 = float4x4({0, 3.001, 0, 0}, {1.999, 0, 0, 0}, {0, 0, 2.001, 0}, {0, 0, 0, 1.001}); + float4x4 m3 = float4x4({0, 3.001, 0, 0}, {1, 1, 0, 0}, {0, 0, 2.001, 0}, {0, 0, 0, 1.001}); + float4x4 m4 = float4x4({0, 1, 0, 0}, {1, 0, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}); + float4x4 m5 = float4x4({0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}); + float4x4 m6 = float4x4({1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}); + EXPECT_TRUE(is_equal(m1, m2, 0.01f)); + EXPECT_FALSE(is_equal(m1, m2, 0.0001f)); + EXPECT_FALSE(is_equal(m1, m3, 0.01f)); + EXPECT_TRUE(is_orthogonal(m1)); + EXPECT_FALSE(is_orthogonal(m3)); + EXPECT_TRUE(is_orthonormal(m4)); + EXPECT_FALSE(is_orthonormal(m1)); + EXPECT_FALSE(is_orthonormal(m3)); + EXPECT_FALSE(is_uniformly_scaled(m1)); + EXPECT_TRUE(is_uniformly_scaled(m4)); + EXPECT_FALSE(is_zero(m4)); + EXPECT_TRUE(is_zero(m5)); + EXPECT_TRUE(is_negative(m4)); + EXPECT_FALSE(is_negative(m5)); + EXPECT_FALSE(is_negative(m6)); +} + +TEST(math_matrix, MatrixMethods) +{ + float4x4 m = float4x4({0, 3, 0, 0}, {2, 0, 0, 0}, {0, 0, 2, 0}, {0, 1, 0, 1}); + auto expect_eul = EulerXYZ(0, 0, M_PI_2); + auto expect_qt = Quaternion(0, -M_SQRT1_2, M_SQRT1_2, 0); + float3 expect_scale = float3(3, 2, 2); + float3 expect_location = float3(0, 1, 0); + + EXPECT_V3_NEAR(float3(to_euler(m)), float3(expect_eul), 0.0002f); + EXPECT_V4_NEAR(float4(to_quaternion(m)), float4(expect_qt), 0.0002f); + EXPECT_EQ(to_scale(m), expect_scale); + + float4 expect_sz = {3, 2, 2, M_SQRT2}; + float4 size; + float4x4 m1 = normalize_and_get_size(m, size); + EXPECT_TRUE(is_unit_scale(m1)); + EXPECT_V4_NEAR(size, expect_sz, 0.0002f); + + float4x4 m2 = normalize(m); + EXPECT_TRUE(is_unit_scale(m2)); + + EulerXYZ eul; + Quaternion qt; + float3 scale; + to_rot_scale(float3x3(m), eul, scale); + to_rot_scale(float3x3(m), qt, scale); + EXPECT_V3_NEAR(scale, expect_scale, 0.00001f); + EXPECT_V4_NEAR(float4(qt), float4(expect_qt), 0.0002f); + EXPECT_V3_NEAR(float3(eul), float3(expect_eul), 0.0002f); + + float3 loc; + to_loc_rot_scale(m, loc, eul, scale); + to_loc_rot_scale(m, loc, qt, scale); + EXPECT_V3_NEAR(scale, expect_scale, 0.00001f); + EXPECT_V3_NEAR(loc, expect_location, 0.00001f); + EXPECT_V4_NEAR(float4(qt), float4(expect_qt), 0.0002f); + EXPECT_V3_NEAR(float3(eul), float3(expect_eul), 0.0002f); +} + +TEST(math_matrix, MatrixTranspose) +{ + float4x4 m({1, 2, 3, 4}, {5, 6, 7, 8}, {9, 1, 2, 3}, {2, 5, 6, 7}); + float4x4 expect({1, 5, 9, 2}, {2, 6, 1, 5}, {3, 7, 2, 6}, {4, 8, 3, 7}); + EXPECT_EQ(transpose(m), expect); +} + +TEST(math_matrix, MatrixInterpolationRegular) +{ + /* Test 4x4 matrix interpolation without singularity, i.e. without axis flip. */ + + /* Transposed matrix, so that the code here is written in the same way as print_m4() outputs. */ + /* This matrix represents T=(0.1, 0.2, 0.3), R=(40, 50, 60) degrees, S=(0.7, 0.8, 0.9) */ + float4x4 m2 = transpose(float4x4({0.224976f, -0.333770f, 0.765074f, 0.100000f}, + {0.389669f, 0.647565f, 0.168130f, 0.200000f}, + {-0.536231f, 0.330541f, 0.443163f, 0.300000f}, + {0.000000f, 0.000000f, 0.000000f, 1.000000f})); + float4x4 m1 = float4x4::identity(); + float4x4 result; + const float epsilon = 1e-6; + result = interpolate(m1, m2, 0.0f); + EXPECT_M4_NEAR(result, m1, epsilon); + result = interpolate(m1, m2, 1.0f); + EXPECT_M4_NEAR(result, m2, epsilon); + + /* This matrix is based on the current implementation of the code, and isn't guaranteed to be + * correct. It's just consistent with the current implementation. */ + float4x4 expect = transpose(float4x4({0.690643f, -0.253244f, 0.484996f, 0.050000f}, + {0.271924f, 0.852623f, 0.012348f, 0.100000f}, + {-0.414209f, 0.137484f, 0.816778f, 0.150000f}, + {0.000000f, 0.000000f, 0.000000f, 1.000000f})); + result = interpolate(m1, m2, 0.5f); + EXPECT_M4_NEAR(result, expect, epsilon); + + result = interpolate_fast(m1, m2, 0.5f); + EXPECT_M4_NEAR(result, expect, epsilon); +} + +TEST(math_matrix, MatrixInterpolationSingularity) +{ + /* A singularity means that there is an axis mirror in the rotation component of the matrix. + * This is reflected in its negative determinant. + * + * The interpolation of 4x4 matrices performs linear interpolation on the translation component, + * and then uses the 3x3 interpolation function to handle rotation and scale. As a result, this + * test for a singularity in the rotation matrix only needs to test the 3x3 case. */ + + /* Transposed matrix, so that the code here is written in the same way as print_m4() outputs. */ + /* This matrix represents R=(4, 5, 6) degrees, S=(-1, 1, 1) */ + float3x3 matrix_a = transpose(float3x3({-0.990737f, -0.098227f, 0.093759f}, + {-0.104131f, 0.992735f, -0.060286f}, + {0.087156f, 0.069491f, 0.993768f})); + EXPECT_NEAR(-1.0f, determinant(matrix_a), 1e-6); + + /* This matrix represents R=(0, 0, 0), S=(-1, 1 1) */ + float3x3 matrix_b = transpose( + float3x3({-1.0f, 0.0f, 0.0f}, {0.0f, 1.0f, 0.0f}, {0.0f, 0.0f, 1.0f})); + + float3x3 result = interpolate(matrix_a, matrix_b, 0.0f); + EXPECT_M3_NEAR(result, matrix_a, 1e-5); + + result = interpolate(matrix_a, matrix_b, 1.0f); + EXPECT_M3_NEAR(result, matrix_b, 1e-5); + + result = interpolate(matrix_a, matrix_b, 0.5f); + + float3x3 expect = transpose(float3x3({-0.997681f, -0.049995f, 0.046186f}, + {-0.051473f, 0.998181f, -0.031385f}, + {0.044533f, 0.033689f, 0.998440f})); + EXPECT_M3_NEAR(result, expect, 1e-5); + + result = interpolate_fast(matrix_a, matrix_b, 0.5f); + EXPECT_M3_NEAR(result, expect, 1e-5); + + /* Interpolating between a matrix with and without axis flip can cause it to go through a zero + * point. The determinant det(A) of a matrix represents the change in volume; interpolating + * between matrices with det(A)=-1 and det(B)=1 will have to go through a point where + * det(result)=0, so where the volume becomes zero. */ + float3x3 matrix_i = float3x3::identity(); + expect = float3x3::zero(); + result = interpolate(matrix_a, matrix_i, 0.5f); + EXPECT_NEAR(0.0f, determinant(result), 1e-5); + EXPECT_M3_NEAR(result, expect, 1e-5); +} + +TEST(math_matrix, MatrixTransform) +{ + float3 expect, result; + const float3 p(1, 2, 3); + float4x4 m4 = from_loc_rot({10, 0, 0}, EulerXYZ(M_PI_2, M_PI_2, M_PI_2)); + float3x3 m3 = from_rotation(EulerXYZ(M_PI_2, M_PI_2, M_PI_2)); + float4x4 pers4 = projection::perspective(-0.1f, 0.1f, -0.1f, 0.1f, -0.1f, -1.0f); + float3x3 pers3 = float3x3({1, 0, 0.1f}, {0, 1, 0.1f}, {0, 0.1f, 1}); + + expect = {13, 2, -1}; + result = transform_point(m4, p); + EXPECT_V3_NEAR(result, expect, 1e-2); + + expect = {3, 2, -1}; + result = transform_point(m3, p); + EXPECT_V3_NEAR(result, expect, 1e-5); + + result = transform_direction(m4, p); + EXPECT_V3_NEAR(result, expect, 1e-5); + + result = transform_direction(m3, p); + EXPECT_V3_NEAR(result, expect, 1e-5); + + expect = {-0.5, -1, -1.7222222}; + result = project_point(pers4, p); + EXPECT_V3_NEAR(result, expect, 1e-5); + + float2 expect2 = {0.76923, 1.61538}; + float2 result2 = project_point(pers3, float2(p)); + EXPECT_V2_NEAR(result2, expect2, 1e-5); +} + +TEST(math_matrix, MatrixProjection) +{ + using namespace math::projection; + float4x4 expect; + float4x4 ortho = orthographic(-0.2f, 0.3f, -0.2f, 0.4f, -0.2f, -0.5f); + float4x4 pers1 = perspective(-0.2f, 0.3f, -0.2f, 0.4f, -0.2f, -0.5f); + float4x4 pers2 = perspective_fov( + math::atan(-0.2f), math::atan(0.3f), math::atan(-0.2f), math::atan(0.4f), -0.2f, -0.5f); + + expect = transpose(float4x4({4.0f, 0.0f, 0.0f, -0.2f}, + {0.0f, 3.33333f, 0.0f, -0.333333f}, + {0.0f, 0.0f, 6.66667f, -2.33333f}, + {0.0f, 0.0f, 0.0f, 1.0f})); + EXPECT_M4_NEAR(ortho, expect, 1e-5); + + expect = transpose(float4x4({-0.8f, 0.0f, 0.2f, 0.0f}, + {0.0f, -0.666667f, 0.333333f, 0.0f}, + {0.0f, 0.0f, -2.33333f, 0.666667f}, + {0.0f, 0.0f, -1.0f, 1.0f})); + EXPECT_M4_NEAR(pers1, expect, 1e-5); + + expect = transpose(float4x4({4.0f, 0.0f, 0.2f, 0.0f}, + {0.0f, 3.33333f, 0.333333f, 0.0f}, + {0.0f, 0.0f, -2.33333f, 0.666667f}, + {0.0f, 0.0f, -1.0f, 1.0f})); + EXPECT_M4_NEAR(pers2, expect, 1e-5); +} + +} // namespace blender::tests diff --git a/source/blender/blenlib/tests/BLI_math_matrix_types_test.cc b/source/blender/blenlib/tests/BLI_math_matrix_types_test.cc new file mode 100644 index 00000000000..9b76c7aec44 --- /dev/null +++ b/source/blender/blenlib/tests/BLI_math_matrix_types_test.cc @@ -0,0 +1,403 @@ +/* SPDX-License-Identifier: Apache-2.0 */ + +#include "testing/testing.h" + +#include "BLI_math_matrix.hh" +#include "BLI_math_matrix_types.hh" + +namespace blender::tests { + +using namespace blender::math; + +TEST(math_matrix_types, DefaultConstructor) +{ + float2x2 m{}; + EXPECT_EQ(m[0][0], 0.0f); + EXPECT_EQ(m[1][1], 0.0f); + EXPECT_EQ(m[0][1], 0.0f); + EXPECT_EQ(m[1][0], 0.0f); +} + +TEST(math_matrix_types, StaticConstructor) +{ + float2x2 m = float2x2::identity(); + EXPECT_EQ(m[0][0], 1.0f); + EXPECT_EQ(m[1][1], 1.0f); + EXPECT_EQ(m[0][1], 0.0f); + EXPECT_EQ(m[1][0], 0.0f); + + m = float2x2::zero(); + EXPECT_EQ(m[0][0], 0.0f); + EXPECT_EQ(m[1][1], 0.0f); + EXPECT_EQ(m[0][1], 0.0f); + EXPECT_EQ(m[1][0], 0.0f); + + m = float2x2::diagonal(2); + EXPECT_EQ(m[0][0], 2.0f); + EXPECT_EQ(m[1][1], 2.0f); + EXPECT_EQ(m[0][1], 0.0f); + EXPECT_EQ(m[1][0], 0.0f); + + m = float2x2::all(1); + EXPECT_EQ(m[0][0], 1.0f); + EXPECT_EQ(m[1][1], 1.0f); + EXPECT_EQ(m[0][1], 1.0f); + EXPECT_EQ(m[1][0], 1.0f); +} + +TEST(math_matrix_types, VectorConstructor) +{ + float3x2 m({1.0f, 2.0f}, {3.0f, 4.0f}, {5.0f, 6.0f}); + EXPECT_EQ(m[0][0], 1.0f); + EXPECT_EQ(m[0][1], 2.0f); + EXPECT_EQ(m[1][0], 3.0f); + EXPECT_EQ(m[1][1], 4.0f); + EXPECT_EQ(m[2][0], 5.0f); + EXPECT_EQ(m[2][1], 6.0f); +} + +TEST(math_matrix_types, SmallerMatrixConstructor) +{ + float2x2 m2({1.0f, 2.0f}, {3.0f, 4.0f}); + float3x3 m3(m2); + EXPECT_EQ(m3[0][0], 1.0f); + EXPECT_EQ(m3[0][1], 2.0f); + EXPECT_EQ(m3[0][2], 0.0f); + EXPECT_EQ(m3[1][0], 3.0f); + EXPECT_EQ(m3[1][1], 4.0f); + EXPECT_EQ(m3[1][2], 0.0f); + EXPECT_EQ(m3[2][0], 0.0f); + EXPECT_EQ(m3[2][1], 0.0f); + EXPECT_EQ(m3[2][2], 1.0f); +} + +TEST(math_matrix_types, ComponentMasking) +{ + float3x3 m3({1.1f, 1.2f, 1.3f}, {2.1f, 2.2f, 2.3f}, {3.1f, 3.2f, 3.3f}); + float2x2 m2(m3); + EXPECT_EQ(m2[0][0], 1.1f); + EXPECT_EQ(m2[0][1], 1.2f); + EXPECT_EQ(m2[1][0], 2.1f); + EXPECT_EQ(m2[1][1], 2.2f); +} + +TEST(math_matrix_types, PointerConversion) +{ + float array[4] = {1.0f, 2.0f, 3.0f, 4.0f}; + float2x2 m2(array); + EXPECT_EQ(m2[0][0], 1.0f); + EXPECT_EQ(m2[0][1], 2.0f); + EXPECT_EQ(m2[1][0], 3.0f); + EXPECT_EQ(m2[1][1], 4.0f); +} + +TEST(math_matrix_types, TypeConversion) +{ + float3x2 m(double3x2({1.0f, 2.0f}, {3.0f, 4.0f}, {5.0f, 6.0f})); + EXPECT_EQ(m[0][0], 1.0f); + EXPECT_EQ(m[0][1], 2.0f); + EXPECT_EQ(m[1][0], 3.0f); + EXPECT_EQ(m[1][1], 4.0f); + EXPECT_EQ(m[2][0], 5.0f); + EXPECT_EQ(m[2][1], 6.0f); + + double3x2 d(m); + EXPECT_EQ(d[0][0], 1.0f); + EXPECT_EQ(d[0][1], 2.0f); + EXPECT_EQ(d[1][0], 3.0f); + EXPECT_EQ(d[1][1], 4.0f); + EXPECT_EQ(d[2][0], 5.0f); + EXPECT_EQ(d[2][1], 6.0f); +} + +TEST(math_matrix_types, PointerArrayConversion) +{ + float array[2][2] = {{1.0f, 2.0f}, {3.0f, 4.0f}}; + float(*ptr)[2] = array; + float2x2 m2(ptr); + EXPECT_EQ(m2[0][0], 1.0f); + EXPECT_EQ(m2[0][1], 2.0f); + EXPECT_EQ(m2[1][0], 3.0f); + EXPECT_EQ(m2[1][1], 4.0f); +} + +TEST(math_matrix_types, ComponentAccess) +{ + float3x3 m3({1.1f, 1.2f, 1.3f}, {2.1f, 2.2f, 2.3f}, {3.1f, 3.2f, 3.3f}); + EXPECT_EQ(m3.x.x, 1.1f); + EXPECT_EQ(m3.x.y, 1.2f); + EXPECT_EQ(m3.y.x, 2.1f); + EXPECT_EQ(m3.y.y, 2.2f); +} + +TEST(math_matrix_types, AddOperator) +{ + float3x3 m3({1.1f, 1.2f, 1.3f}, {2.1f, 2.2f, 2.3f}, {3.1f, 3.2f, 3.3f}); + + m3 = m3 + float3x3::diagonal(2); + EXPECT_EQ(m3[0][0], 3.1f); + EXPECT_EQ(m3[0][2], 1.3f); + EXPECT_EQ(m3[2][0], 3.1f); + EXPECT_EQ(m3[2][2], 5.3f); + + m3 += float3x3::diagonal(-1.0f); + EXPECT_EQ(m3[0][0], 2.1f); + EXPECT_EQ(m3[0][2], 1.3f); + EXPECT_EQ(m3[2][0], 3.1f); + EXPECT_EQ(m3[2][2], 4.3f); + + m3 += 1.0f; + EXPECT_EQ(m3[0][0], 3.1f); + EXPECT_EQ(m3[0][2], 2.3f); + EXPECT_EQ(m3[2][0], 4.1f); + EXPECT_EQ(m3[2][2], 5.3f); + + m3 = m3 + 1.0f; + EXPECT_EQ(m3[0][0], 4.1f); + EXPECT_EQ(m3[0][2], 3.3f); + EXPECT_EQ(m3[2][0], 5.1f); + EXPECT_EQ(m3[2][2], 6.3f); + + m3 = 1.0f + m3; + EXPECT_EQ(m3[0][0], 5.1f); + EXPECT_EQ(m3[0][2], 4.3f); + EXPECT_EQ(m3[2][0], 6.1f); + EXPECT_EQ(m3[2][2], 7.3f); +} + +TEST(math_matrix_types, SubtractOperator) +{ + float3x3 m3({10.0f, 10.2f, 10.3f}, {20.1f, 20.2f, 20.3f}, {30.1f, 30.2f, 30.3f}); + + m3 = m3 - float3x3::diagonal(2); + EXPECT_EQ(m3[0][0], 8.0f); + EXPECT_EQ(m3[0][2], 10.3f); + EXPECT_EQ(m3[2][0], 30.1f); + EXPECT_EQ(m3[2][2], 28.3f); + + m3 -= float3x3::diagonal(-1.0f); + EXPECT_EQ(m3[0][0], 9.0f); + EXPECT_EQ(m3[0][2], 10.3f); + EXPECT_EQ(m3[2][0], 30.1f); + EXPECT_EQ(m3[2][2], 29.3f); + + m3 -= 1.0f; + EXPECT_EQ(m3[0][0], 8.0f); + EXPECT_EQ(m3[0][2], 9.3f); + EXPECT_EQ(m3[2][0], 29.1f); + EXPECT_EQ(m3[2][2], 28.3f); + + m3 = m3 - 1.0f; + EXPECT_EQ(m3[0][0], 7.0f); + EXPECT_EQ(m3[0][2], 8.3f); + EXPECT_EQ(m3[2][0], 28.1f); + EXPECT_EQ(m3[2][2], 27.3f); + + m3 = 1.0f - m3; + EXPECT_EQ(m3[0][0], -6.0f); + EXPECT_EQ(m3[0][2], -7.3f); + EXPECT_EQ(m3[2][0], -27.1f); + EXPECT_EQ(m3[2][2], -26.3f); +} + +TEST(math_matrix_types, MultiplyOperator) +{ + float3x3 m3(float3(1.0f), float3(2.0f), float3(2.0f)); + + m3 = m3 * 2; + EXPECT_EQ(m3[0][0], 2.0f); + EXPECT_EQ(m3[2][2], 4.0f); + + m3 = 2 * m3; + EXPECT_EQ(m3[0][0], 4.0f); + EXPECT_EQ(m3[2][2], 8.0f); + + m3 *= 2; + EXPECT_EQ(m3[0][0], 8.0f); + EXPECT_EQ(m3[2][2], 16.0f); +} + +TEST(math_matrix_types, MatrixMultiplyOperator) +{ + float2x2 a(float2(1, 2), float2(3, 4)); + float2x2 b(float2(5, 6), float2(7, 8)); + + float2x2 result = a * b; + EXPECT_EQ(result[0][0], 23); + EXPECT_EQ(result[0][1], 34); + EXPECT_EQ(result[1][0], 31); + EXPECT_EQ(result[1][1], 46); + + result = a; + result *= b; + EXPECT_EQ(result[0][0], 23); + EXPECT_EQ(result[0][1], 34); + EXPECT_EQ(result[1][0], 31); + EXPECT_EQ(result[1][1], 46); + + /* Test SSE2 implementation. */ + float4x4 result2 = float4x4::diagonal(2) * float4x4::diagonal(6); + EXPECT_EQ(result2, float4x4::diagonal(12)); + + float3x3 result3 = float3x3::diagonal(2) * float3x3::diagonal(6); + EXPECT_EQ(result3, float3x3::diagonal(12)); + + /* Non square matrices. */ + float3x2 a4(float2(1, 2), float2(3, 4), float2(5, 6)); + float2x3 b4(float3(11, 7, 5), float3(13, 11, 17)); + + float2x2 expect4(float2(57, 80), float2(131, 172)); + + float2x2 result4 = a4 * b4; + EXPECT_EQ(result4[0][0], expect4[0][0]); + EXPECT_EQ(result4[0][1], expect4[0][1]); + EXPECT_EQ(result4[1][0], expect4[1][0]); + EXPECT_EQ(result4[1][1], expect4[1][1]); +} + +TEST(math_matrix_types, VectorMultiplyOperator) +{ + float3x2 mat(float2(1, 2), float2(3, 4), float2(5, 6)); + + float2 result = mat * float3(7, 8, 9); + EXPECT_EQ(result[0], 76); + EXPECT_EQ(result[1], 100); + + float3 result2 = float2(2, 3) * mat; + EXPECT_EQ(result2[0], 8); + EXPECT_EQ(result2[1], 18); + EXPECT_EQ(result2[2], 28); +} + +TEST(math_matrix_types, ViewConstructor) +{ + float4x4 mat = float4x4( + float4(1, 2, 3, 4), float4(5, 6, 7, 8), float4(9, 10, 11, 12), float4(13, 14, 15, 16)); + + auto view = mat.view<2, 2, 1, 1>(); + EXPECT_EQ(view[0][0], 6); + EXPECT_EQ(view[0][1], 7); + EXPECT_EQ(view[1][0], 10); + EXPECT_EQ(view[1][1], 11); + + float2x2 center = view; + EXPECT_EQ(center[0][0], 6); + EXPECT_EQ(center[0][1], 7); + EXPECT_EQ(center[1][0], 10); + EXPECT_EQ(center[1][1], 11); +} + +TEST(math_matrix_types, ViewFromCstyleMatrix) +{ + float c_style_mat[4][4] = {{1, 2, 3, 4}, {5, 6, 7, 8}, {9, 10, 11, 12}, {13, 14, 15, 16}}; + float4x4_view c_mat_view = float4x4_view(c_style_mat); + + float4x4_mutableview c_mat_mutable_view = float4x4_mutableview(c_style_mat); + + float4x4 expect = float4x4({2, 4, 6, 8}, {10, 12, 14, 16}, {18, 20, 22, 24}, {26, 28, 30, 32}); + + float4x4 mat = float4x4::diagonal(2.0f) * c_mat_view; + EXPECT_M4_NEAR(expect, mat, 1e-8f); + + c_mat_mutable_view *= float4x4::diagonal(2.0f); + EXPECT_M4_NEAR(expect, c_mat_mutable_view, 1e-8f); +} + +TEST(math_matrix_types, ViewAssignment) +{ + float4x4 mat = float4x4( + float4(1, 2, 3, 4), float4(5, 6, 7, 8), float4(9, 10, 11, 12), float4(13, 14, 15, 16)); + + mat.view<2, 2, 1, 1>() = float2x2({-1, -2}, {-3, -4}); + + float4x4 expect = float4x4({1, 2, 3, 4}, {5, -1, -2, 8}, {9, -3, -4, 12}, {13, 14, 15, 16}); + EXPECT_M4_NEAR(expect, mat, 1e-8f); + + /* Test view-view assignment. */ + mat.view<2, 2, 2, 2>() = mat.view<2, 2, 0, 0>(); + float4x4 expect2 = float4x4({1, 2, 3, 4}, {5, -1, -2, 8}, {9, -3, 1, 2}, {13, 14, 5, -1}); + EXPECT_M4_NEAR(expect2, mat, 1e-8f); + + mat.view<2, 2, 0, 0>() = mat.view<2, 2, 1, 1>(); + float4x4 expect3 = float4x4({-1, -2, 3, 4}, {-3, 1, -2, 8}, {9, -3, 1, 2}, {13, 14, 5, -1}); + EXPECT_M4_NEAR(expect3, mat, 1e-8f); + + /* Should fail to compile. */ + // const float4x4 &mat_const = mat; + // mat.view<2, 2, 2, 2>() = mat_const.view<2, 2, 0, 0>(); + + /* Should fail to run. */ + // mat.view<2, 2, 1, 1>() = mat.view<2, 2>(); +} + +TEST(math_matrix_types, ViewScalarOperators) +{ + float4x4 mat = float4x4({1, 2, 3, 4}, {5, 6, 7, 8}, {9, 10, 11, 12}, {13, 14, 15, 16}); + + auto view = mat.view<2, 2, 1, 1>(); + EXPECT_EQ(view[0][0], 6); + EXPECT_EQ(view[0][1], 7); + EXPECT_EQ(view[1][0], 10); + EXPECT_EQ(view[1][1], 11); + + view += 1; + EXPECT_EQ(view[0][0], 7); + EXPECT_EQ(view[0][1], 8); + EXPECT_EQ(view[1][0], 11); + EXPECT_EQ(view[1][1], 12); + + view -= 2; + EXPECT_EQ(view[0][0], 5); + EXPECT_EQ(view[0][1], 6); + EXPECT_EQ(view[1][0], 9); + EXPECT_EQ(view[1][1], 10); + + view *= 4; + EXPECT_EQ(view[0][0], 20); + EXPECT_EQ(view[0][1], 24); + EXPECT_EQ(view[1][0], 36); + EXPECT_EQ(view[1][1], 40); + + /* Since we modified the view, we expect the source to have changed. */ + float4x4 expect = float4x4({1, 2, 3, 4}, {5, 20, 24, 8}, {9, 36, 40, 12}, {13, 14, 15, 16}); + EXPECT_M4_NEAR(expect, mat, 1e-8f); + + view = -view; + EXPECT_EQ(view[0][0], -20); + EXPECT_EQ(view[0][1], -24); + EXPECT_EQ(view[1][0], -36); + EXPECT_EQ(view[1][1], -40); +} + +TEST(math_matrix_types, ViewMatrixMultiplyOperator) +{ + float4x4 mat = float4x4({1, 2, 3, 4}, {5, 6, 7, 8}, {9, 10, 11, 12}, {13, 14, 15, 16}); + auto view = mat.view<2, 2, 1, 1>(); + view = float2x2({1, 2}, {3, 4}); + + float2x2 result = view * float2x2({5, 6}, {7, 8}); + EXPECT_EQ(result[0][0], 23); + EXPECT_EQ(result[0][1], 34); + EXPECT_EQ(result[1][0], 31); + EXPECT_EQ(result[1][1], 46); + + view *= float2x2({5, 6}, {7, 8}); + EXPECT_EQ(view[0][0], 23); + EXPECT_EQ(view[0][1], 34); + EXPECT_EQ(view[1][0], 31); + EXPECT_EQ(view[1][1], 46); +} + +TEST(math_matrix_types, ViewMatrixNormalize) +{ + float4x4 mat = float4x4({1, 2, 3, 4}, {5, 6, 7, 8}, {9, 10, 11, 12}, {13, 14, 15, 16}); + mat.view<3, 3>() = normalize(mat.view<3, 3>()); + + float4x4 expect = float4x4({0.267261236, 0.534522473, 0.80178368, 4}, + {0.476731300, 0.572077572, 0.66742378, 8}, + {0.517891824, 0.575435340, 0.63297885, 12}, + {13, 14, 15, 16}); + EXPECT_M4_NEAR(expect, mat, 1e-8f); +} + +} // namespace blender::tests diff --git a/source/blender/blenlib/tests/BLI_math_rotation_test.cc b/source/blender/blenlib/tests/BLI_math_rotation_test.cc index 7f4cbbd8b8c..32d8f8bf19e 100644 --- a/source/blender/blenlib/tests/BLI_math_rotation_test.cc +++ b/source/blender/blenlib/tests/BLI_math_rotation_test.cc @@ -5,6 +5,7 @@ #include "BLI_math_base.h" #include "BLI_math_matrix.h" #include "BLI_math_rotation.h" +#include "BLI_math_rotation.hh" #include "BLI_math_rotation_legacy.hh" #include "BLI_math_vector.hh" @@ -271,6 +272,20 @@ TEST(math_rotation, sin_cos_from_fraction_symmetry) namespace blender::math::tests { +TEST(math_rotation, DefaultConstructor) +{ + Quaternion quat{}; + EXPECT_EQ(quat.x, 0.0f); + EXPECT_EQ(quat.y, 0.0f); + EXPECT_EQ(quat.z, 0.0f); + EXPECT_EQ(quat.w, 0.0f); + + EulerXYZ eul{}; + EXPECT_EQ(eul.x, 0.0f); + EXPECT_EQ(eul.y, 0.0f); + EXPECT_EQ(eul.z, 0.0f); +} + TEST(math_rotation, RotateDirectionAroundAxis) { const float3 a = rotate_direction_around_axis({1, 0, 0}, {0, 0, 1}, M_PI_2); @@ -287,4 +302,41 @@ TEST(math_rotation, RotateDirectionAroundAxis) EXPECT_NEAR(c.z, 1.0f, FLT_EPSILON); } +TEST(math_rotation, AxisAngleConstructors) +{ + AxisAngle a({0.0f, 0.0f, 2.0f}, M_PI_2); + EXPECT_V3_NEAR(a.axis(), float3(0, 0, 1), 1e-4); + EXPECT_NEAR(a.angle(), M_PI_2, 1e-4); + + AxisAngleNormalized b({0.0f, 0.0f, 1.0f}, M_PI_2); + EXPECT_V3_NEAR(b.axis(), float3(0, 0, 1), 1e-4); + EXPECT_NEAR(b.angle(), M_PI_2, 1e-4); + + AxisAngle c({1.0f, 0.0f, 0.0f}, {0.0f, 1.0f, 0.0f}); + EXPECT_V3_NEAR(c.axis(), float3(0, 0, 1), 1e-4); + EXPECT_NEAR(c.angle(), M_PI_2, 1e-4); + + AxisAngle d({1.0f, 0.0f, 0.0f}, {0.0f, -1.0f, 0.0f}); + EXPECT_V3_NEAR(d.axis(), float3(0, 0, -1), 1e-4); + EXPECT_NEAR(d.angle(), M_PI_2, 1e-4); +} + +TEST(math_rotation, TypeConversion) +{ + EulerXYZ euler(0, 0, M_PI_2); + Quaternion quat(M_SQRT1_2, 0.0f, 0.0f, M_SQRT1_2); + AxisAngle axis_angle({0.0f, 0.0f, 2.0f}, M_PI_2); + + EXPECT_V4_NEAR(float4(Quaternion(euler)), float4(quat), 1e-4); + EXPECT_V3_NEAR(AxisAngle(euler).axis(), axis_angle.axis(), 1e-4); + EXPECT_NEAR(AxisAngle(euler).angle(), axis_angle.angle(), 1e-4); + + EXPECT_V3_NEAR(float3(EulerXYZ(quat)), float3(euler), 1e-4); + EXPECT_V3_NEAR(AxisAngle(quat).axis(), axis_angle.axis(), 1e-4); + EXPECT_NEAR(AxisAngle(quat).angle(), axis_angle.angle(), 1e-4); + + EXPECT_V3_NEAR(float3(EulerXYZ(axis_angle)), float3(euler), 1e-4); + EXPECT_V4_NEAR(float4(Quaternion(axis_angle)), float4(quat), 1e-4); +} + } // namespace blender::math::tests diff --git a/tests/gtests/testing/testing.h b/tests/gtests/testing/testing.h index 7b3e6e667aa..0e47dd802cb 100644 --- a/tests/gtests/testing/testing.h +++ b/tests/gtests/testing/testing.h @@ -42,6 +42,12 @@ const std::string &flags_test_release_dir(); /* bin/{blender version} in the bui } \ (void)0 +#define EXPECT_M2_NEAR(a, b, eps) \ + do { \ + EXPECT_V2_NEAR(a[0], b[0], eps); \ + EXPECT_V2_NEAR(a[1], b[1], eps); \ + } while (false); + #define EXPECT_M3_NEAR(a, b, eps) \ do { \ EXPECT_V3_NEAR(a[0], b[0], eps); \