From f0500a28ee205e50f2849fbfc6a96e835e407322 Mon Sep 17 00:00:00 2001
From: Jacques Lucke <jacques@blender.org>
Date: Tue, 3 Dec 2024 12:24:55 +0100
Subject: [PATCH] Fix: BLI: quaternion is not normalized after conversion

This applies the fix from 98334b8f7df333abbab63e1aaf340ad8cd9255a9 to the c++ implementation.

Two notes:
* We need to work towards unifying these implementations which would have avoided this bug.
* In the C++ implementation, one can't use `math::dot` and `math::normalize` for quaternions
  where this function is implemented, because of include dependency order.

Both these things should be resolved, but right now correctness has priority for me.

Pull Request: https://projects.blender.org/blender/blender/pulls/131296
---
 source/blender/blenlib/BLI_math_matrix.hh | 15 +++++++++++++++
 1 file changed, 15 insertions(+)

diff --git a/source/blender/blenlib/BLI_math_matrix.hh b/source/blender/blenlib/BLI_math_matrix.hh
index 4ebdd487be3..9b33fb38440 100644
--- a/source/blender/blenlib/BLI_math_matrix.hh
+++ b/source/blender/blenlib/BLI_math_matrix.hh
@@ -924,6 +924,21 @@ template<typename T> QuaternionBase<T> normalized_to_quat_fast(const MatBase<T,
   }
 
   BLI_assert(!(q.w < 0.0f));
+
+  /* Sometimes normalization is necessary due to round-off errors in the above
+   * calculations. The comparison here uses tighter tolerances than
+   * BLI_ASSERT_UNIT_QUAT(), so it's likely that even after a few more
+   * transformations the quaternion will still be considered unit-ish. */
+  const T q_len_squared = q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
+  const T threshold = 0.0002f /* #BLI_ASSERT_UNIT_EPSILON */ * 3;
+  if (math::abs(q_len_squared - 1.0f) >= threshold) {
+    const T q_len_inv = 1.0 / math::sqrt(q_len_squared);
+    q.x *= q_len_inv;
+    q.y *= q_len_inv;
+    q.z *= q_len_inv;
+    q.w *= q_len_inv;
+  }
+
   BLI_assert(math::is_unit_scale(VecBase<T, 4>(q)));
   return q;
 }