From 3980cb3844a6eb67f409d23224f17733db8d3176 Mon Sep 17 00:00:00 2001 From: korenkonder Date: Sun, 16 Jul 2023 11:59:48 +0300 Subject: [PATCH] Fixed `mat4_blend`, `quat_from_mat3`, `quat::slerp`, added `quat::lerp` --- src/KKdLib/mat.cpp | 14 +++++++------ src/KKdLib/quat.cpp | 49 +++++++++++++++++++++++---------------------- src/KKdLib/quat.hpp | 46 +++++++++++++++++++++++------------------- 3 files changed, 58 insertions(+), 51 deletions(-) diff --git a/src/KKdLib/mat.cpp b/src/KKdLib/mat.cpp index 755cc49c..f67b2189 100644 --- a/src/KKdLib/mat.cpp +++ b/src/KKdLib/mat.cpp @@ -2130,20 +2130,22 @@ inline void mat4_blend(const mat4* x, const mat4* y, mat4* z, float_t blend) { quat_from_mat3(x->row0.x, x->row1.x, x->row2.x, x->row0.y, x->row1.y, x->row2.y, x->row0.z, x->row1.z, x->row2.z, &q0); + q0 = quat::normalize(q0); quat_from_mat3(y->row0.x, y->row1.x, y->row2.x, y->row0.y, y->row1.y, y->row2.y, y->row0.z, y->row1.z, y->row2.z, &q1); + q1 = quat::normalize(q1); + vec3 t0; vec3 t1; vec3 t2; - vec3 t3; - mat4_get_translation(x, &t1); - mat4_get_translation(y, &t2); + mat4_get_translation(x, &t0); + mat4_get_translation(y, &t1); - q2 = quat::slerp(q0, q1, blend); - t3 = vec3::lerp(t1, t2, blend); + q2 = quat::lerp(q0, q1, blend); + t2 = vec3::lerp(t0, t1, blend); mat4_from_quat(&q2, z); - mat4_set_translation(z, &t3); + mat4_set_translation(z, &t2); } inline void mat4_blend_rotation(const mat4* x, const mat4* y, mat4* z, float_t blend) { diff --git a/src/KKdLib/quat.cpp b/src/KKdLib/quat.cpp index 1b37117c..070154b4 100644 --- a/src/KKdLib/quat.cpp +++ b/src/KKdLib/quat.cpp @@ -39,39 +39,40 @@ inline void quat_mult(const quat* x, const quat* y, quat* z) { void quat_from_mat3(float_t m00, float_t m01, float_t m02, float_t m10, float_t m11, float_t m12, float_t m20, float_t m21, float_t m22, quat* quat) { - float_t sq; - if (m00 + m11 + m22 >= 0.0f) { - sq = sqrtf(m00 + m11 + m22 + 1.0f); - quat->w = 0.5f * sq; + float_t sq = sqrtf(m00 + m11 + m22 + 1.0f); + quat->w = sq * 0.5f; sq = 0.5f / sq; - quat->x = (m12 - m21) * sq; - quat->y = (m20 - m02) * sq; - quat->z = (m01 - m10) * sq; + quat->x = (m21 - m12) * sq; + quat->y = (m02 - m20) * sq; + quat->z = (m10 - m01) * sq; + return; } - else if (m00 > m11 && m00 > m22) { - sq = sqrtf(m00 - m11 - m22 + 1.0f); - quat->x = 0.5f * sq; + + float_t max = max_def(m22, max_def(m11, m00)); + if (max == m00) { + float_t sq = sqrtf(m00 - (m11 + m22) + 1.0f); + quat->x = sq * 0.5f; sq = 0.5f / sq; - quat->y = (m10 + m01) * sq; - quat->z = (m20 + m02) * sq; - quat->w = (m12 - m21) * sq; + quat->y = (m01 + m10) * sq; + quat->z = (m02 + m20) * sq; + quat->w = (m21 - m12) * sq; } - else if (m11 > m22) { - sq = sqrtf(m11 - m00 - m22 + 1.0f); - quat->y = 0.5f * sq; + else if (max == m11) { + float_t sq = sqrtf(m11 - (m00 + m22) + 1.0f); + quat->y = sq * 0.5f; sq = 0.5f / sq; - quat->x = (m10 + m01) * sq; - quat->z = (m21 + m12) * sq; - quat->w = (m20 - m02) * sq; + quat->x = (m01 + m10) * sq; + quat->z = (m12 + m21) * sq; + quat->w = (m02 - m20) * sq; } else { - sq = sqrtf(m22 - m00 - m11 + 1.0f); - quat->z = 0.5f * sq; + float_t sq = sqrtf(m22 - (m00 + m11) + 1.0f); + quat->z = sq * 0.5f; sq = 0.5f / sq; - quat->x = (m20 + m02) * sq; - quat->y = (m21 + m12) * sq; - quat->w = (m01 - m10) * sq; + quat->x = (m02 + m20) * sq; + quat->y = (m12 + m21) * sq; + quat->w = (m10 - m01) * sq; } } diff --git a/src/KKdLib/quat.hpp b/src/KKdLib/quat.hpp index 74a255c2..d6fa34f5 100644 --- a/src/KKdLib/quat.hpp +++ b/src/KKdLib/quat.hpp @@ -19,6 +19,7 @@ struct quat { static float_t length_squared(const quat& left); static float_t distance(const quat& left, const quat& right); static float_t distance_squared(const quat& left, const quat& right); + static quat lerp(const quat& left, const quat& right, const float_t blend); static quat slerp(const quat& left, const quat& right, const float_t blend); static quat normalize(const quat& left); static quat rcp(const quat& left); @@ -177,37 +178,40 @@ inline float_t quat::distance_squared(const quat& left, const quat& right) { return _mm_cvtss_f32(_mm_hadd_ps(zt, zt)); } +inline quat quat::lerp(const quat& left, const quat& right, const float_t blend) { + quat x_t; + quat y_t; + x_t = left; + y_t = right; + + if (quat::dot(x_t, y_t) < 0.0f) + x_t = -x_t; + + return quat::normalize(x_t * (1.0f - blend) + y_t * blend); +} + inline quat quat::slerp(const quat& left, const quat& right, const float_t blend) { quat x_t; quat y_t; - quat z_t; - x_t = quat::normalize(left); - y_t = quat::normalize(right); + x_t = left; + y_t = right; float_t dot = quat::dot(x_t, y_t); if (dot < 0.0f) { - z_t = -y_t; dot = -dot; + x_t = -x_t; } - else - z_t = y_t; - const float_t DOT_THRESHOLD = 0.9995f; - float_t s0, s1; - if (dot <= DOT_THRESHOLD) { - float_t theta_0 = acosf(dot); - float_t theta = theta_0 * blend; - float_t sin_theta = sinf(theta); - float_t sin_theta_0 = sinf(theta_0); + dot = min_def(dot, 1.0f); - s0 = cosf(theta) - dot * sin_theta / sin_theta_0; - s1 = sin_theta / sin_theta_0; - } - else { - s0 = (1.0f - blend); - s1 = blend; - } - return quat::normalize(left * s0 + z_t * s0); + float_t theta = acosf(dot); + if (theta == 0.0f) + return x_t; + + float_t st = 1.0f / sinf(theta); + float_t s0 = sinf((1.0f - blend) * theta) * st; + float_t s1 = sinf(theta * blend) * st; + return quat::normalize(x_t * s0 + y_t * s1); } inline quat quat::normalize(const quat& left) {