Max/korenkonder_ReDIVA
1
0
Fork 0
mirror of https://github.com/korenkonder/ReDIVA.git synced 2026-10-03 04:08:10 +03:00
Code Issues Packages Projects Releases Wiki Activity

Fixed mat4_blend, quat_from_mat3, quat::slerp, added quat::lerp

Browse Source
This commit is contained in:
korenkonder
2023-07-16 11:59:48 +03:00
parent 5e3af33ac6
commit 3980cb3844
3 changed files with 58 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/KKdLib/mat.cpp
+8 -6
View File
@@ -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) {
src/KKdLib/quat.cpp
+25 -24
View File
@@ -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;
}
}
src/KKdLib/quat.hpp
+25 -21
View File
@@ -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) {