From 586b8995ca3f46fdf6e79aa71f7655cc9c69a63a Mon Sep 17 00:00:00 2001
From: korenkonder <admin@mysterious-home.net>
Date: Sat, 18 May 2024 17:41:08 +0300
Subject: [PATCH] Better math for vec and quat

---
 src/KKdLib/quat.hpp |  5 ++++-
 src/KKdLib/vec.hpp  | 16 ++++++++--------
 2 files changed, 12 insertions(+), 9 deletions(-)

diff --git a/src/KKdLib/quat.hpp b/src/KKdLib/quat.hpp
index 24748ce3..3fd0ff8d 100644
--- a/src/KKdLib/quat.hpp
+++ b/src/KKdLib/quat.hpp
@@ -285,9 +285,12 @@ inline quat quat::slerp(const quat& left, const quat& right, const float_t blend
     float_t dot = quat::dot(x_t, y_t);
     if (dot < 0.0f) {
         dot = -dot;
-        x_t = -x_t;
+        y_t = -y_t;
     }
 
+    if (1.0 - dot <= 0.08f)
+        return quat::lerp(x_t, y_t, blend);
+
     dot = min_def(dot, 1.0f);
 
     float_t theta = acosf(dot);
diff --git a/src/KKdLib/vec.hpp b/src/KKdLib/vec.hpp
index f6050873..5d9b0c1e 100644
--- a/src/KKdLib/vec.hpp
+++ b/src/KKdLib/vec.hpp
@@ -754,8 +754,8 @@ inline vec2 vec2::normalize_rcp(const vec2& left) {
     zt = _mm_mul_ps(xt, xt);
     zt = _mm_sqrt_ss(_mm_hadd_ps(zt, zt));
     if (_mm_cvtss_f32(zt) != 0.0f)
-        zt = _mm_div_ss(vec4::load_xmm(1.0f), zt);
-    return vec2::store_xmm(_mm_mul_ps(xt, _mm_shuffle_ps(zt, zt, 0)));
+        return vec2::store_xmm(_mm_mul_ps(xt, _mm_div_ss(vec2::load_xmm(1.0f), _mm_shuffle_ps(zt, zt, 0))));
+    return vec2::store_xmm(xt);
 }
 
 inline vec2 vec2::rcp(const vec2& left) {
@@ -1086,8 +1086,8 @@ inline vec3 vec3::normalize_rcp(const vec3& left) {
     zt = _mm_hadd_ps(zt, zt);
     zt = _mm_sqrt_ss(_mm_hadd_ps(zt, zt));
     if (_mm_cvtss_f32(zt) != 0.0f)
-        zt = _mm_div_ss(vec4::load_xmm(1.0f), zt);
-    return vec3::store_xmm(_mm_mul_ps(xt, _mm_shuffle_ps(zt, zt, 0)));
+        return vec3::store_xmm(_mm_mul_ps(xt, _mm_div_ss(vec3::load_xmm(1.0f), _mm_shuffle_ps(zt, zt, 0))));
+    return vec3::store_xmm(xt);
 }
 
 inline vec3 vec3::rcp(const vec3& left) {
@@ -1421,8 +1421,8 @@ inline vec4 vec4::normalize_rcp(const vec4& left) {
     zt = _mm_hadd_ps(zt, zt);
     zt = _mm_sqrt_ss(_mm_hadd_ps(zt, zt));
     if (_mm_cvtss_f32(zt) != 0.0f)
-        zt = _mm_div_ss(vec4::load_xmm(1.0f), zt);
-    return vec4::store_xmm(_mm_mul_ps(xt, _mm_shuffle_ps(zt, zt, 0)));
+        return vec4::store_xmm(_mm_mul_ps(xt, _mm_div_ss(vec4::load_xmm(1.0f), _mm_shuffle_ps(zt, zt, 0))));
+    return vec4::store_xmm(xt);
 }
 
 inline vec4 vec4::rcp(const vec4& left) {
@@ -1975,8 +1975,8 @@ inline vec2d vec2d::normalize_rcp(const vec2d& left) {
     zt = _mm_mul_pd(xt, xt);
     zt = _mm_sqrt_sd(_mm_hadd_pd(zt, zt), zt);
     if (_mm_cvtsd_f64(zt) != 0.0f)
-        zt = _mm_div_sd(vec2d::load_xmm(1.0), zt);
-    return vec2d::store_xmm(_mm_mul_pd(xt, _mm_shuffle_pd(zt, zt, 0)));
+        return vec2d::store_xmm(_mm_mul_pd(xt, _mm_div_sd(vec2d::load_xmm(1.0f), _mm_shuffle_pd(zt, zt, 0))));
+    return vec2d::store_xmm(xt);
 }
 
 inline vec2d vec2d::rcp(const vec2d& left) {