diff options
Diffstat (limited to 'source/blender/blenlib/intern/math_rotation.c')
| -rw-r--r-- | source/blender/blenlib/intern/math_rotation.c | 2980 |
1 files changed, 1495 insertions, 1485 deletions
diff --git a/source/blender/blenlib/intern/math_rotation.c b/source/blender/blenlib/intern/math_rotation.c index b1bed27d2bd..47814b1080c 100644 --- a/source/blender/blenlib/intern/math_rotation.c +++ b/source/blender/blenlib/intern/math_rotation.c @@ -38,42 +38,42 @@ /* convenience, avoids setting Y axis everywhere */ void unit_axis_angle(float axis[3], float *angle) { - axis[0] = 0.0f; - axis[1] = 1.0f; - axis[2] = 0.0f; - *angle = 0.0f; + axis[0] = 0.0f; + axis[1] = 1.0f; + axis[2] = 0.0f; + *angle = 0.0f; } void unit_qt(float q[4]) { - q[0] = 1.0f; - q[1] = q[2] = q[3] = 0.0f; + q[0] = 1.0f; + q[1] = q[2] = q[3] = 0.0f; } void copy_qt_qt(float q1[4], const float q2[4]) { - q1[0] = q2[0]; - q1[1] = q2[1]; - q1[2] = q2[2]; - q1[3] = q2[3]; + q1[0] = q2[0]; + q1[1] = q2[1]; + q1[2] = q2[2]; + q1[3] = q2[3]; } bool is_zero_qt(const float q[4]) { - return (q[0] == 0 && q[1] == 0 && q[2] == 0 && q[3] == 0); + return (q[0] == 0 && q[1] == 0 && q[2] == 0 && q[3] == 0); } void mul_qt_qtqt(float q[4], const float q1[4], const float q2[4]) { - float t0, t1, t2; + float t0, t1, t2; - t0 = q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]; - t1 = q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]; - t2 = q1[0] * q2[2] + q1[2] * q2[0] + q1[3] * q2[1] - q1[1] * q2[3]; - q[3] = q1[0] * q2[3] + q1[3] * q2[0] + q1[1] * q2[2] - q1[2] * q2[1]; - q[0] = t0; - q[1] = t1; - q[2] = t2; + t0 = q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]; + t1 = q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]; + t2 = q1[0] * q2[2] + q1[2] * q2[0] + q1[3] * q2[1] - q1[1] * q2[3]; + q[3] = q1[0] * q2[3] + q1[3] * q2[0] + q1[1] * q2[2] - q1[2] * q2[1]; + q[0] = t0; + q[1] = t1; + q[2] = t2; } /** @@ -97,58 +97,58 @@ void mul_qt_qtqt(float q[4], const float q1[4], const float q2[4]) */ void mul_qt_v3(const float q[4], float v[3]) { - float t0, t1, t2; + float t0, t1, t2; - t0 = -q[1] * v[0] - q[2] * v[1] - q[3] * v[2]; - t1 = q[0] * v[0] + q[2] * v[2] - q[3] * v[1]; - t2 = q[0] * v[1] + q[3] * v[0] - q[1] * v[2]; - v[2] = q[0] * v[2] + q[1] * v[1] - q[2] * v[0]; - v[0] = t1; - v[1] = t2; + t0 = -q[1] * v[0] - q[2] * v[1] - q[3] * v[2]; + t1 = q[0] * v[0] + q[2] * v[2] - q[3] * v[1]; + t2 = q[0] * v[1] + q[3] * v[0] - q[1] * v[2]; + v[2] = q[0] * v[2] + q[1] * v[1] - q[2] * v[0]; + v[0] = t1; + v[1] = t2; - t1 = t0 * -q[1] + v[0] * q[0] - v[1] * q[3] + v[2] * q[2]; - t2 = t0 * -q[2] + v[1] * q[0] - v[2] * q[1] + v[0] * q[3]; - v[2] = t0 * -q[3] + v[2] * q[0] - v[0] * q[2] + v[1] * q[1]; - v[0] = t1; - v[1] = t2; + t1 = t0 * -q[1] + v[0] * q[0] - v[1] * q[3] + v[2] * q[2]; + t2 = t0 * -q[2] + v[1] * q[0] - v[2] * q[1] + v[0] * q[3]; + v[2] = t0 * -q[3] + v[2] * q[0] - v[0] * q[2] + v[1] * q[1]; + v[0] = t1; + v[1] = t2; } void conjugate_qt_qt(float q1[4], const float q2[4]) { - q1[0] = q2[0]; - q1[1] = -q2[1]; - q1[2] = -q2[2]; - q1[3] = -q2[3]; + q1[0] = q2[0]; + q1[1] = -q2[1]; + q1[2] = -q2[2]; + q1[3] = -q2[3]; } void conjugate_qt(float q[4]) { - q[1] = -q[1]; - q[2] = -q[2]; - q[3] = -q[3]; + q[1] = -q[1]; + q[2] = -q[2]; + q[3] = -q[3]; } float dot_qtqt(const float q1[4], const float q2[4]) { - return q1[0] * q2[0] + q1[1] * q2[1] + q1[2] * q2[2] + q1[3] * q2[3]; + return q1[0] * q2[0] + q1[1] * q2[1] + q1[2] * q2[2] + q1[3] * q2[3]; } void invert_qt(float q[4]) { - const float f = dot_qtqt(q, q); + const float f = dot_qtqt(q, q); - if (f == 0.0f) { - return; - } + if (f == 0.0f) { + return; + } - conjugate_qt(q); - mul_qt_fl(q, 1.0f / f); + conjugate_qt(q); + mul_qt_fl(q, 1.0f / f); } void invert_qt_qt(float q1[4], const float q2[4]) { - copy_qt_qt(q1, q2); - invert_qt(q1); + copy_qt_qt(q1, q2); + invert_qt(q1); } /** @@ -158,48 +158,47 @@ void invert_qt_qt(float q1[4], const float q2[4]) */ void invert_qt_normalized(float q[4]) { - BLI_ASSERT_UNIT_QUAT(q); - conjugate_qt(q); - + BLI_ASSERT_UNIT_QUAT(q); + conjugate_qt(q); } void invert_qt_qt_normalized(float q1[4], const float q2[4]) { - copy_qt_qt(q1, q2); - invert_qt_normalized(q1); + copy_qt_qt(q1, q2); + invert_qt_normalized(q1); } /* simple mult */ void mul_qt_fl(float q[4], const float f) { - q[0] *= f; - q[1] *= f; - q[2] *= f; - q[3] *= f; + q[0] *= f; + q[1] *= f; + q[2] *= f; + q[3] *= f; } void sub_qt_qtqt(float q[4], const float q1[4], const float q2[4]) { - float nq2[4]; + float nq2[4]; - nq2[0] = -q2[0]; - nq2[1] = q2[1]; - nq2[2] = q2[2]; - nq2[3] = q2[3]; + nq2[0] = -q2[0]; + nq2[1] = q2[1]; + nq2[2] = q2[2]; + nq2[3] = q2[3]; - mul_qt_qtqt(q, q1, nq2); + mul_qt_qtqt(q, q1, nq2); } /* raise a unit quaternion to the specified power */ void pow_qt_fl_normalized(float q[4], const float fac) { - BLI_ASSERT_UNIT_QUAT(q); - const float angle = fac * saacos(q[0]); /* quat[0] = cos(0.5 * angle), - * but now the 0.5 and 2.0 rule out */ - const float co = cosf(angle); - const float si = sinf(angle); - q[0] = co; - normalize_v3_length(q + 1, si); + BLI_ASSERT_UNIT_QUAT(q); + const float angle = fac * saacos(q[0]); /* quat[0] = cos(0.5 * angle), + * but now the 0.5 and 2.0 rule out */ + const float co = cosf(angle); + const float si = sinf(angle); + q[0] = co; + normalize_v3_length(q + 1, si); } /** @@ -208,248 +207,252 @@ void pow_qt_fl_normalized(float q[4], const float fac) */ void quat_to_compatible_quat(float q[4], const float a[4], const float old[4]) { - const float eps = 1e-4f; - BLI_ASSERT_UNIT_QUAT(a); - float old_unit[4]; - /* Skips `!finite_v4(old)` case too. */ - if (normalize_qt_qt(old_unit, old) > eps) { - float delta[4]; - rotation_between_quats_to_quat(delta, old_unit, a); - mul_qt_qtqt(q, old, delta); - if ((q[0] < 0.0f) != (old[0] < 0.0f)) { - negate_v4(q); - } - } - else { - copy_qt_qt(q, a); - } + const float eps = 1e-4f; + BLI_ASSERT_UNIT_QUAT(a); + float old_unit[4]; + /* Skips `!finite_v4(old)` case too. */ + if (normalize_qt_qt(old_unit, old) > eps) { + float delta[4]; + rotation_between_quats_to_quat(delta, old_unit, a); + mul_qt_qtqt(q, old, delta); + if ((q[0] < 0.0f) != (old[0] < 0.0f)) { + negate_v4(q); + } + } + else { + copy_qt_qt(q, a); + } } /* skip error check, currently only needed by mat3_to_quat_is_ok */ static void quat_to_mat3_no_error(float m[3][3], const float q[4]) { - double q0, q1, q2, q3, qda, qdb, qdc, qaa, qab, qac, qbb, qbc, qcc; + double q0, q1, q2, q3, qda, qdb, qdc, qaa, qab, qac, qbb, qbc, qcc; - q0 = M_SQRT2 * (double)q[0]; - q1 = M_SQRT2 * (double)q[1]; - q2 = M_SQRT2 * (double)q[2]; - q3 = M_SQRT2 * (double)q[3]; + q0 = M_SQRT2 * (double)q[0]; + q1 = M_SQRT2 * (double)q[1]; + q2 = M_SQRT2 * (double)q[2]; + q3 = M_SQRT2 * (double)q[3]; - qda = q0 * q1; - qdb = q0 * q2; - qdc = q0 * q3; - qaa = q1 * q1; - qab = q1 * q2; - qac = q1 * q3; - qbb = q2 * q2; - qbc = q2 * q3; - qcc = q3 * q3; + qda = q0 * q1; + qdb = q0 * q2; + qdc = q0 * q3; + qaa = q1 * q1; + qab = q1 * q2; + qac = q1 * q3; + qbb = q2 * q2; + qbc = q2 * q3; + qcc = q3 * q3; - m[0][0] = (float)(1.0 - qbb - qcc); - m[0][1] = (float)(qdc + qab); - m[0][2] = (float)(-qdb + qac); + m[0][0] = (float)(1.0 - qbb - qcc); + m[0][1] = (float)(qdc + qab); + m[0][2] = (float)(-qdb + qac); - m[1][0] = (float)(-qdc + qab); - m[1][1] = (float)(1.0 - qaa - qcc); - m[1][2] = (float)(qda + qbc); + m[1][0] = (float)(-qdc + qab); + m[1][1] = (float)(1.0 - qaa - qcc); + m[1][2] = (float)(qda + qbc); - m[2][0] = (float)(qdb + qac); - m[2][1] = (float)(-qda + qbc); - m[2][2] = (float)(1.0 - qaa - qbb); + m[2][0] = (float)(qdb + qac); + m[2][1] = (float)(-qda + qbc); + m[2][2] = (float)(1.0 - qaa - qbb); } void quat_to_mat3(float m[3][3], const float q[4]) { #ifdef DEBUG - float f; - if (!((f = dot_qtqt(q, q)) == 0.0f || (fabsf(f - 1.0f) < (float)QUAT_EPSILON))) { - fprintf(stderr, "Warning! quat_to_mat3() called with non-normalized: size %.8f *** report a bug ***\n", f); - } + float f; + if (!((f = dot_qtqt(q, q)) == 0.0f || (fabsf(f - 1.0f) < (float)QUAT_EPSILON))) { + fprintf(stderr, + "Warning! quat_to_mat3() called with non-normalized: size %.8f *** report a bug ***\n", + f); + } #endif - quat_to_mat3_no_error(m, q); + quat_to_mat3_no_error(m, q); } void quat_to_mat4(float m[4][4], const float q[4]) { - double q0, q1, q2, q3, qda, qdb, qdc, qaa, qab, qac, qbb, qbc, qcc; + double q0, q1, q2, q3, qda, qdb, qdc, qaa, qab, qac, qbb, qbc, qcc; #ifdef DEBUG - if (!((q0 = dot_qtqt(q, q)) == 0.0 || (fabs(q0 - 1.0) < QUAT_EPSILON))) { - fprintf(stderr, "Warning! quat_to_mat4() called with non-normalized: size %.8f *** report a bug ***\n", (float)q0); - } + if (!((q0 = dot_qtqt(q, q)) == 0.0 || (fabs(q0 - 1.0) < QUAT_EPSILON))) { + fprintf(stderr, + "Warning! quat_to_mat4() called with non-normalized: size %.8f *** report a bug ***\n", + (float)q0); + } #endif - q0 = M_SQRT2 * (double)q[0]; - q1 = M_SQRT2 * (double)q[1]; - q2 = M_SQRT2 * (double)q[2]; - q3 = M_SQRT2 * (double)q[3]; + q0 = M_SQRT2 * (double)q[0]; + q1 = M_SQRT2 * (double)q[1]; + q2 = M_SQRT2 * (double)q[2]; + q3 = M_SQRT2 * (double)q[3]; - qda = q0 * q1; - qdb = q0 * q2; - qdc = q0 * q3; - qaa = q1 * q1; - qab = q1 * q2; - qac = q1 * q3; - qbb = q2 * q2; - qbc = q2 * q3; - qcc = q3 * q3; + qda = q0 * q1; + qdb = q0 * q2; + qdc = q0 * q3; + qaa = q1 * q1; + qab = q1 * q2; + qac = q1 * q3; + qbb = q2 * q2; + qbc = q2 * q3; + qcc = q3 * q3; - m[0][0] = (float)(1.0 - qbb - qcc); - m[0][1] = (float)(qdc + qab); - m[0][2] = (float)(-qdb + qac); - m[0][3] = 0.0f; + m[0][0] = (float)(1.0 - qbb - qcc); + m[0][1] = (float)(qdc + qab); + m[0][2] = (float)(-qdb + qac); + m[0][3] = 0.0f; - m[1][0] = (float)(-qdc + qab); - m[1][1] = (float)(1.0 - qaa - qcc); - m[1][2] = (float)(qda + qbc); - m[1][3] = 0.0f; + m[1][0] = (float)(-qdc + qab); + m[1][1] = (float)(1.0 - qaa - qcc); + m[1][2] = (float)(qda + qbc); + m[1][3] = 0.0f; - m[2][0] = (float)(qdb + qac); - m[2][1] = (float)(-qda + qbc); - m[2][2] = (float)(1.0 - qaa - qbb); - m[2][3] = 0.0f; + m[2][0] = (float)(qdb + qac); + m[2][1] = (float)(-qda + qbc); + m[2][2] = (float)(1.0 - qaa - qbb); + m[2][3] = 0.0f; - m[3][0] = m[3][1] = m[3][2] = 0.0f; - m[3][3] = 1.0f; + m[3][0] = m[3][1] = m[3][2] = 0.0f; + m[3][3] = 1.0f; } void mat3_normalized_to_quat(float q[4], const float mat[3][3]) { - double tr, s; - - BLI_ASSERT_UNIT_M3(mat); - - tr = 0.25 * (double)(1.0f + mat[0][0] + mat[1][1] + mat[2][2]); - - if (tr > (double)1e-4f) { - s = sqrt(tr); - q[0] = (float)s; - s = 1.0 / (4.0 * s); - q[1] = (float)((double)(mat[1][2] - mat[2][1]) * s); - q[2] = (float)((double)(mat[2][0] - mat[0][2]) * s); - q[3] = (float)((double)(mat[0][1] - mat[1][0]) * s); - } - else { - if (mat[0][0] > mat[1][1] && mat[0][0] > mat[2][2]) { - s = 2.0f * sqrtf(1.0f + mat[0][0] - mat[1][1] - mat[2][2]); - q[1] = (float)(0.25 * s); - - s = 1.0 / s; - q[0] = (float)((double)(mat[1][2] - mat[2][1]) * s); - q[2] = (float)((double)(mat[1][0] + mat[0][1]) * s); - q[3] = (float)((double)(mat[2][0] + mat[0][2]) * s); - } - else if (mat[1][1] > mat[2][2]) { - s = 2.0f * sqrtf(1.0f + mat[1][1] - mat[0][0] - mat[2][2]); - q[2] = (float)(0.25 * s); - - s = 1.0 / s; - q[0] = (float)((double)(mat[2][0] - mat[0][2]) * s); - q[1] = (float)((double)(mat[1][0] + mat[0][1]) * s); - q[3] = (float)((double)(mat[2][1] + mat[1][2]) * s); - } - else { - s = 2.0f * sqrtf(1.0f + mat[2][2] - mat[0][0] - mat[1][1]); - q[3] = (float)(0.25 * s); - - s = 1.0 / s; - q[0] = (float)((double)(mat[0][1] - mat[1][0]) * s); - q[1] = (float)((double)(mat[2][0] + mat[0][2]) * s); - q[2] = (float)((double)(mat[2][1] + mat[1][2]) * s); - } - } - - normalize_qt(q); + double tr, s; + + BLI_ASSERT_UNIT_M3(mat); + + tr = 0.25 * (double)(1.0f + mat[0][0] + mat[1][1] + mat[2][2]); + + if (tr > (double)1e-4f) { + s = sqrt(tr); + q[0] = (float)s; + s = 1.0 / (4.0 * s); + q[1] = (float)((double)(mat[1][2] - mat[2][1]) * s); + q[2] = (float)((double)(mat[2][0] - mat[0][2]) * s); + q[3] = (float)((double)(mat[0][1] - mat[1][0]) * s); + } + else { + if (mat[0][0] > mat[1][1] && mat[0][0] > mat[2][2]) { + s = 2.0f * sqrtf(1.0f + mat[0][0] - mat[1][1] - mat[2][2]); + q[1] = (float)(0.25 * s); + + s = 1.0 / s; + q[0] = (float)((double)(mat[1][2] - mat[2][1]) * s); + q[2] = (float)((double)(mat[1][0] + mat[0][1]) * s); + q[3] = (float)((double)(mat[2][0] + mat[0][2]) * s); + } + else if (mat[1][1] > mat[2][2]) { + s = 2.0f * sqrtf(1.0f + mat[1][1] - mat[0][0] - mat[2][2]); + q[2] = (float)(0.25 * s); + + s = 1.0 / s; + q[0] = (float)((double)(mat[2][0] - mat[0][2]) * s); + q[1] = (float)((double)(mat[1][0] + mat[0][1]) * s); + q[3] = (float)((double)(mat[2][1] + mat[1][2]) * s); + } + else { + s = 2.0f * sqrtf(1.0f + mat[2][2] - mat[0][0] - mat[1][1]); + q[3] = (float)(0.25 * s); + + s = 1.0 / s; + q[0] = (float)((double)(mat[0][1] - mat[1][0]) * s); + q[1] = (float)((double)(mat[2][0] + mat[0][2]) * s); + q[2] = (float)((double)(mat[2][1] + mat[1][2]) * s); + } + } + + normalize_qt(q); } void mat3_to_quat(float q[4], const float m[3][3]) { - float unit_mat[3][3]; + float unit_mat[3][3]; - /* work on a copy */ - /* this is needed AND a 'normalize_qt' in the end */ - normalize_m3_m3(unit_mat, m); - mat3_normalized_to_quat(q, unit_mat); + /* work on a copy */ + /* this is needed AND a 'normalize_qt' in the end */ + normalize_m3_m3(unit_mat, m); + mat3_normalized_to_quat(q, unit_mat); } void mat4_normalized_to_quat(float q[4], const float m[4][4]) { - float mat3[3][3]; + float mat3[3][3]; - copy_m3_m4(mat3, m); - mat3_normalized_to_quat(q, mat3); + copy_m3_m4(mat3, m); + mat3_normalized_to_quat(q, mat3); } void mat4_to_quat(float q[4], const float m[4][4]) { - float mat3[3][3]; + float mat3[3][3]; - copy_m3_m4(mat3, m); - mat3_to_quat(q, mat3); + copy_m3_m4(mat3, m); + mat3_to_quat(q, mat3); } void mat3_to_quat_is_ok(float q[4], const float wmat[3][3]) { - float mat[3][3], matr[3][3], matn[3][3], q1[4], q2[4], angle, si, co, nor[3]; + float mat[3][3], matr[3][3], matn[3][3], q1[4], q2[4], angle, si, co, nor[3]; - /* work on a copy */ - copy_m3_m3(mat, wmat); - normalize_m3(mat); + /* work on a copy */ + copy_m3_m3(mat, wmat); + normalize_m3(mat); - /* rotate z-axis of matrix to z-axis */ + /* rotate z-axis of matrix to z-axis */ - nor[0] = mat[2][1]; /* cross product with (0,0,1) */ - nor[1] = -mat[2][0]; - nor[2] = 0.0; - normalize_v3(nor); + nor[0] = mat[2][1]; /* cross product with (0,0,1) */ + nor[1] = -mat[2][0]; + nor[2] = 0.0; + normalize_v3(nor); - co = mat[2][2]; - angle = 0.5f * saacos(co); + co = mat[2][2]; + angle = 0.5f * saacos(co); - co = cosf(angle); - si = sinf(angle); - q1[0] = co; - q1[1] = -nor[0] * si; /* negative here, but why? */ - q1[2] = -nor[1] * si; - q1[3] = -nor[2] * si; + co = cosf(angle); + si = sinf(angle); + q1[0] = co; + q1[1] = -nor[0] * si; /* negative here, but why? */ + q1[2] = -nor[1] * si; + q1[3] = -nor[2] * si; - /* rotate back x-axis from mat, using inverse q1 */ - quat_to_mat3_no_error(matr, q1); - invert_m3_m3(matn, matr); - mul_m3_v3(matn, mat[0]); + /* rotate back x-axis from mat, using inverse q1 */ + quat_to_mat3_no_error(matr, q1); + invert_m3_m3(matn, matr); + mul_m3_v3(matn, mat[0]); - /* and align x-axes */ - angle = 0.5f * atan2f(mat[0][1], mat[0][0]); + /* and align x-axes */ + angle = 0.5f * atan2f(mat[0][1], mat[0][0]); - co = cosf(angle); - si = sinf(angle); - q2[0] = co; - q2[1] = 0.0f; - q2[2] = 0.0f; - q2[3] = si; + co = cosf(angle); + si = sinf(angle); + q2[0] = co; + q2[1] = 0.0f; + q2[2] = 0.0f; + q2[3] = si; - mul_qt_qtqt(q, q1, q2); + mul_qt_qtqt(q, q1, q2); } float normalize_qt(float q[4]) { - const float len = sqrtf(dot_qtqt(q, q)); + const float len = sqrtf(dot_qtqt(q, q)); - if (len != 0.0f) { - mul_qt_fl(q, 1.0f / len); - } - else { - q[1] = 1.0f; - q[0] = q[2] = q[3] = 0.0f; - } + if (len != 0.0f) { + mul_qt_fl(q, 1.0f / len); + } + else { + q[1] = 1.0f; + q[0] = q[2] = q[3] = 0.0f; + } - return len; + return len; } float normalize_qt_qt(float r[4], const float q[4]) { - copy_qt_qt(r, q); - return normalize_qt(r); + copy_qt_qt(r, q); + return normalize_qt(r); } /** @@ -457,82 +460,81 @@ float normalize_qt_qt(float r[4], const float q[4]) */ void rotation_between_vecs_to_mat3(float m[3][3], const float v1[3], const float v2[3]) { - float axis[3]; - /* avoid calculating the angle */ - float angle_sin; - float angle_cos; - - BLI_ASSERT_UNIT_V3(v1); - BLI_ASSERT_UNIT_V3(v2); - - cross_v3_v3v3(axis, v1, v2); - - angle_sin = normalize_v3(axis); - angle_cos = dot_v3v3(v1, v2); - - if (angle_sin > FLT_EPSILON) { -axis_calc: - BLI_ASSERT_UNIT_V3(axis); - axis_angle_normalized_to_mat3_ex(m, axis, angle_sin, angle_cos); - BLI_ASSERT_UNIT_M3(m); - } - else { - if (angle_cos > 0.0f) { - /* Same vectors, zero rotation... */ - unit_m3(m); - } - else { - /* Colinear but opposed vectors, 180 rotation... */ - ortho_v3_v3(axis, v1); - normalize_v3(axis); - angle_sin = 0.0f; /* sin(M_PI) */ - angle_cos = -1.0f; /* cos(M_PI) */ - goto axis_calc; - } - } + float axis[3]; + /* avoid calculating the angle */ + float angle_sin; + float angle_cos; + + BLI_ASSERT_UNIT_V3(v1); + BLI_ASSERT_UNIT_V3(v2); + + cross_v3_v3v3(axis, v1, v2); + + angle_sin = normalize_v3(axis); + angle_cos = dot_v3v3(v1, v2); + + if (angle_sin > FLT_EPSILON) { + axis_calc: + BLI_ASSERT_UNIT_V3(axis); + axis_angle_normalized_to_mat3_ex(m, axis, angle_sin, angle_cos); + BLI_ASSERT_UNIT_M3(m); + } + else { + if (angle_cos > 0.0f) { + /* Same vectors, zero rotation... */ + unit_m3(m); + } + else { + /* Colinear but opposed vectors, 180 rotation... */ + ortho_v3_v3(axis, v1); + normalize_v3(axis); + angle_sin = 0.0f; /* sin(M_PI) */ + angle_cos = -1.0f; /* cos(M_PI) */ + goto axis_calc; + } + } } /* note: expects vectors to be normalized */ void rotation_between_vecs_to_quat(float q[4], const float v1[3], const float v2[3]) { - float axis[3]; + float axis[3]; - cross_v3_v3v3(axis, v1, v2); + cross_v3_v3v3(axis, v1, v2); - if (normalize_v3(axis) > FLT_EPSILON) { - float angle; + if (normalize_v3(axis) > FLT_EPSILON) { + float angle; - angle = angle_normalized_v3v3(v1, v2); + angle = angle_normalized_v3v3(v1, v2); - axis_angle_normalized_to_quat(q, axis, angle); - } - else { - /* degenerate case */ + axis_angle_normalized_to_quat(q, axis, angle); + } + else { + /* degenerate case */ - if (dot_v3v3(v1, v2) > 0.0f) { - /* Same vectors, zero rotation... */ - unit_qt(q); - } - else { - /* Colinear but opposed vectors, 180 rotation... */ - ortho_v3_v3(axis, v1); - axis_angle_to_quat(q, axis, (float)M_PI); - } - } + if (dot_v3v3(v1, v2) > 0.0f) { + /* Same vectors, zero rotation... */ + unit_qt(q); + } + else { + /* Colinear but opposed vectors, 180 rotation... */ + ortho_v3_v3(axis, v1); + axis_angle_to_quat(q, axis, (float)M_PI); + } + } } void rotation_between_quats_to_quat(float q[4], const float q1[4], const float q2[4]) { - float tquat[4]; + float tquat[4]; - conjugate_qt_qt(tquat, q1); + conjugate_qt_qt(tquat, q1); - mul_qt_fl(tquat, 1.0f / dot_qtqt(tquat, tquat)); + mul_qt_fl(tquat, 1.0f / dot_qtqt(tquat, tquat)); - mul_qt_qtqt(q, tquat, q2); + mul_qt_qtqt(q, tquat, q2); } - /* -------------------------------------------------------------------- */ /** \name Quaternion Angle * @@ -543,39 +545,39 @@ void rotation_between_quats_to_quat(float q[4], const float q1[4], const float q float angle_normalized_qt(const float q[4]) { - BLI_ASSERT_UNIT_QUAT(q); - return 2.0f * saacos(q[0]); + BLI_ASSERT_UNIT_QUAT(q); + return 2.0f * saacos(q[0]); } float angle_qt(const float q[4]) { - float tquat[4]; + float tquat[4]; - normalize_qt_qt(tquat, q); + normalize_qt_qt(tquat, q); - return angle_normalized_qt(tquat); + return angle_normalized_qt(tquat); } float angle_normalized_qtqt(const float q1[4], const float q2[4]) { - float qdelta[4]; + float qdelta[4]; - BLI_ASSERT_UNIT_QUAT(q1); - BLI_ASSERT_UNIT_QUAT(q2); + BLI_ASSERT_UNIT_QUAT(q1); + BLI_ASSERT_UNIT_QUAT(q2); - rotation_between_quats_to_quat(qdelta, q1, q2); + rotation_between_quats_to_quat(qdelta, q1, q2); - return angle_normalized_qt(qdelta); + return angle_normalized_qt(qdelta); } float angle_qtqt(const float q1[4], const float q2[4]) { - float quat1[4], quat2[4]; + float quat1[4], quat2[4]; - normalize_qt_qt(quat1, q1); - normalize_qt_qt(quat2, q2); + normalize_qt_qt(quat1, q1); + normalize_qt_qt(quat2, q2); - return angle_normalized_qtqt(quat1, quat2); + return angle_normalized_qtqt(quat1, quat2); } /** \} */ @@ -592,148 +594,160 @@ float angle_qtqt(const float q1[4], const float q2[4]) float angle_signed_normalized_qt(const float q[4]) { - BLI_ASSERT_UNIT_QUAT(q); - if (q[0] >= 0.0f) { - return 2.0f * saacos(q[0]); - } - else { - return -2.0f * saacos(-q[0]); - } + BLI_ASSERT_UNIT_QUAT(q); + if (q[0] >= 0.0f) { + return 2.0f * saacos(q[0]); + } + else { + return -2.0f * saacos(-q[0]); + } } float angle_signed_normalized_qtqt(const float q1[4], const float q2[4]) { - if (dot_qtqt(q1, q2) >= 0.0f) { - return angle_normalized_qtqt(q1, q2); - } - else { - float q2_copy[4]; - negate_v4_v4(q2_copy, q2); - return -angle_normalized_qtqt(q1, q2_copy); - } + if (dot_qtqt(q1, q2) >= 0.0f) { + return angle_normalized_qtqt(q1, q2); + } + else { + float q2_copy[4]; + negate_v4_v4(q2_copy, q2); + return -angle_normalized_qtqt(q1, q2_copy); + } } float angle_signed_qt(const float q[4]) { - float tquat[4]; + float tquat[4]; - normalize_qt_qt(tquat, q); + normalize_qt_qt(tquat, q); - return angle_signed_normalized_qt(tquat); + return angle_signed_normalized_qt(tquat); } float angle_signed_qtqt(const float q1[4], const float q2[4]) { - if (dot_qtqt(q1, q2) >= 0.0f) { - return angle_qtqt(q1, q2); - } - else { - float q2_copy[4]; - negate_v4_v4(q2_copy, q2); - return -angle_qtqt(q1, q2_copy); - } + if (dot_qtqt(q1, q2) >= 0.0f) { + return angle_qtqt(q1, q2); + } + else { + float q2_copy[4]; + negate_v4_v4(q2_copy, q2); + return -angle_qtqt(q1, q2_copy); + } } /** \} */ void vec_to_quat(float q[4], const float vec[3], short axis, const short upflag) { - const float eps = 1e-4f; - float nor[3], tvec[3]; - float angle, si, co, len; - - assert(axis >= 0 && axis <= 5); - assert(upflag >= 0 && upflag <= 2); - - /* first set the quat to unit */ - unit_qt(q); - - len = len_v3(vec); - - if (UNLIKELY(len == 0.0f)) { - return; - } - - /* rotate to axis */ - if (axis > 2) { - copy_v3_v3(tvec, vec); - axis = (short)(axis - 3); - } - else { - negate_v3_v3(tvec, vec); - } - - /* nasty! I need a good routine for this... - * problem is a rotation of an Y axis to the negative Y-axis for example. - */ - - if (axis == 0) { /* x-axis */ - nor[0] = 0.0; - nor[1] = -tvec[2]; - nor[2] = tvec[1]; - - if (fabsf(tvec[1]) + fabsf(tvec[2]) < eps) { - nor[1] = 1.0f; - } - - co = tvec[0]; - } - else if (axis == 1) { /* y-axis */ - nor[0] = tvec[2]; - nor[1] = 0.0; - nor[2] = -tvec[0]; - - if (fabsf(tvec[0]) + fabsf(tvec[2]) < eps) { - nor[2] = 1.0f; - } - - co = tvec[1]; - } - else { /* z-axis */ - nor[0] = -tvec[1]; - nor[1] = tvec[0]; - nor[2] = 0.0; - - if (fabsf(tvec[0]) + fabsf(tvec[1]) < eps) { - nor[0] = 1.0f; - } - - co = tvec[2]; - } - co /= len; - - normalize_v3(nor); - - axis_angle_normalized_to_quat(q, nor, saacos(co)); - - if (axis != upflag) { - float mat[3][3]; - float q2[4]; - const float *fp = mat[2]; - quat_to_mat3(mat, q); - - if (axis == 0) { - if (upflag == 1) { angle = 0.5f * atan2f(fp[2], fp[1]); } - else { angle = -0.5f * atan2f(fp[1], fp[2]); } - } - else if (axis == 1) { - if (upflag == 0) { angle = -0.5f * atan2f(fp[2], fp[0]); } - else { angle = 0.5f * atan2f(fp[0], fp[2]); } - } - else { - if (upflag == 0) { angle = 0.5f * atan2f(-fp[1], -fp[0]); } - else { angle = -0.5f * atan2f(-fp[0], -fp[1]); } - } - - co = cosf(angle); - si = sinf(angle) / len; - q2[0] = co; - q2[1] = tvec[0] * si; - q2[2] = tvec[1] * si; - q2[3] = tvec[2] * si; - - mul_qt_qtqt(q, q2, q); - } + const float eps = 1e-4f; + float nor[3], tvec[3]; + float angle, si, co, len; + + assert(axis >= 0 && axis <= 5); + assert(upflag >= 0 && upflag <= 2); + + /* first set the quat to unit */ + unit_qt(q); + + len = len_v3(vec); + + if (UNLIKELY(len == 0.0f)) { + return; + } + + /* rotate to axis */ + if (axis > 2) { + copy_v3_v3(tvec, vec); + axis = (short)(axis - 3); + } + else { + negate_v3_v3(tvec, vec); + } + + /* nasty! I need a good routine for this... + * problem is a rotation of an Y axis to the negative Y-axis for example. + */ + + if (axis == 0) { /* x-axis */ + nor[0] = 0.0; + nor[1] = -tvec[2]; + nor[2] = tvec[1]; + + if (fabsf(tvec[1]) + fabsf(tvec[2]) < eps) { + nor[1] = 1.0f; + } + + co = tvec[0]; + } + else if (axis == 1) { /* y-axis */ + nor[0] = tvec[2]; + nor[1] = 0.0; + nor[2] = -tvec[0]; + + if (fabsf(tvec[0]) + fabsf(tvec[2]) < eps) { + nor[2] = 1.0f; + } + + co = tvec[1]; + } + else { /* z-axis */ + nor[0] = -tvec[1]; + nor[1] = tvec[0]; + nor[2] = 0.0; + + if (fabsf(tvec[0]) + fabsf(tvec[1]) < eps) { + nor[0] = 1.0f; + } + + co = tvec[2]; + } + co /= len; + + normalize_v3(nor); + + axis_angle_normalized_to_quat(q, nor, saacos(co)); + + if (axis != upflag) { + float mat[3][3]; + float q2[4]; + const float *fp = mat[2]; + quat_to_mat3(mat, q); + + if (axis == 0) { + if (upflag == 1) { + angle = 0.5f * atan2f(fp[2], fp[1]); + } + else { + angle = -0.5f * atan2f(fp[1], fp[2]); + } + } + else if (axis == 1) { + if (upflag == 0) { + angle = -0.5f * atan2f(fp[2], fp[0]); + } + else { + angle = 0.5f * atan2f(fp[0], fp[2]); + } + } + else { + if (upflag == 0) { + angle = 0.5f * atan2f(-fp[1], -fp[0]); + } + else { + angle = -0.5f * atan2f(-fp[0], -fp[1]); + } + } + + co = cosf(angle); + si = sinf(angle) / len; + q2[0] = co; + q2[1] = tvec[0] * si; + q2[2] = tvec[1] * si; + q2[3] = tvec[2] * si; + + mul_qt_qtqt(q, q2, q); + } } #if 0 @@ -741,42 +755,42 @@ void vec_to_quat(float q[4], const float vec[3], short axis, const short upflag) /* A & M Watt, Advanced animation and rendering techniques, 1992 ACM press */ void QuatInterpolW(float *result, float quat1[4], float quat2[4], float t) { - float omega, cosom, sinom, sc1, sc2; - - cosom = quat1[0] * quat2[0] + quat1[1] * quat2[1] + quat1[2] * quat2[2] + quat1[3] * quat2[3]; - - /* rotate around shortest angle */ - if ((1.0f + cosom) > 0.0001f) { - - if ((1.0f - cosom) > 0.0001f) { - omega = (float)acos(cosom); - sinom = sinf(omega); - sc1 = sinf((1.0 - t) * omega) / sinom; - sc2 = sinf(t * omega) / sinom; - } - else { - sc1 = 1.0f - t; - sc2 = t; - } - result[0] = sc1 * quat1[0] + sc2 * quat2[0]; - result[1] = sc1 * quat1[1] + sc2 * quat2[1]; - result[2] = sc1 * quat1[2] + sc2 * quat2[2]; - result[3] = sc1 * quat1[3] + sc2 * quat2[3]; - } - else { - result[0] = quat2[3]; - result[1] = -quat2[2]; - result[2] = quat2[1]; - result[3] = -quat2[0]; - - sc1 = sinf((1.0 - t) * M_PI_2); - sc2 = sinf(t * M_PI_2); - - result[0] = sc1 * quat1[0] + sc2 * result[0]; - result[1] = sc1 * quat1[1] + sc2 * result[1]; - result[2] = sc1 * quat1[2] + sc2 * result[2]; - result[3] = sc1 * quat1[3] + sc2 * result[3]; - } + float omega, cosom, sinom, sc1, sc2; + + cosom = quat1[0] * quat2[0] + quat1[1] * quat2[1] + quat1[2] * quat2[2] + quat1[3] * quat2[3]; + + /* rotate around shortest angle */ + if ((1.0f + cosom) > 0.0001f) { + + if ((1.0f - cosom) > 0.0001f) { + omega = (float)acos(cosom); + sinom = sinf(omega); + sc1 = sinf((1.0 - t) * omega) / sinom; + sc2 = sinf(t * omega) / sinom; + } + else { + sc1 = 1.0f - t; + sc2 = t; + } + result[0] = sc1 * quat1[0] + sc2 * quat2[0]; + result[1] = sc1 * quat1[1] + sc2 * quat2[1]; + result[2] = sc1 * quat1[2] + sc2 * quat2[2]; + result[3] = sc1 * quat1[3] + sc2 * quat2[3]; + } + else { + result[0] = quat2[3]; + result[1] = -quat2[2]; + result[2] = quat2[1]; + result[3] = -quat2[0]; + + sc1 = sinf((1.0 - t) * M_PI_2); + sc2 = sinf(t * M_PI_2); + + result[0] = sc1 * quat1[0] + sc2 * result[0]; + result[1] = sc1 * quat1[1] + sc2 * result[1]; + result[2] = sc1 * quat1[2] + sc2 * result[2]; + result[3] = sc1 * quat1[3] + sc2 * result[3]; + } } #endif @@ -790,112 +804,112 @@ void QuatInterpolW(float *result, float quat1[4], float quat2[4], float t) */ void interp_dot_slerp(const float t, const float cosom, float r_w[2]) { - const float eps = 1e-4f; + const float eps = 1e-4f; - BLI_assert(IN_RANGE_INCL(cosom, -1.0001f, 1.0001f)); + BLI_assert(IN_RANGE_INCL(cosom, -1.0001f, 1.0001f)); - /* within [-1..1] range, avoid aligned axis */ - if (LIKELY(fabsf(cosom) < (1.0f - eps))) { - float omega, sinom; + /* within [-1..1] range, avoid aligned axis */ + if (LIKELY(fabsf(cosom) < (1.0f - eps))) { + float omega, sinom; - omega = acosf(cosom); - sinom = sinf(omega); - r_w[0] = sinf((1.0f - t) * omega) / sinom; - r_w[1] = sinf(t * omega) / sinom; - } - else { - /* fallback to lerp */ - r_w[0] = 1.0f - t; - r_w[1] = t; - } + omega = acosf(cosom); + sinom = sinf(omega); + r_w[0] = sinf((1.0f - t) * omega) / sinom; + r_w[1] = sinf(t * omega) / sinom; + } + else { + /* fallback to lerp */ + r_w[0] = 1.0f - t; + r_w[1] = t; + } } void interp_qt_qtqt(float result[4], const float quat1[4], const float quat2[4], const float t) { - float quat[4], cosom, w[2]; + float quat[4], cosom, w[2]; - BLI_ASSERT_UNIT_QUAT(quat1); - BLI_ASSERT_UNIT_QUAT(quat2); + BLI_ASSERT_UNIT_QUAT(quat1); + BLI_ASSERT_UNIT_QUAT(quat2); - cosom = dot_qtqt(quat1, quat2); + cosom = dot_qtqt(quat1, quat2); - /* rotate around shortest angle */ - if (cosom < 0.0f) { - cosom = -cosom; - negate_v4_v4(quat, quat1); - } - else { - copy_qt_qt(quat, quat1); - } + /* rotate around shortest angle */ + if (cosom < 0.0f) { + cosom = -cosom; + negate_v4_v4(quat, quat1); + } + else { + copy_qt_qt(quat, quat1); + } - interp_dot_slerp(t, cosom, w); + interp_dot_slerp(t, cosom, w); - result[0] = w[0] * quat[0] + w[1] * quat2[0]; - result[1] = w[0] * quat[1] + w[1] * quat2[1]; - result[2] = w[0] * quat[2] + w[1] * quat2[2]; - result[3] = w[0] * quat[3] + w[1] * quat2[3]; + result[0] = w[0] * quat[0] + w[1] * quat2[0]; + result[1] = w[0] * quat[1] + w[1] * quat2[1]; + result[2] = w[0] * quat[2] + w[1] * quat2[2]; + result[3] = w[0] * quat[3] + w[1] * quat2[3]; } void add_qt_qtqt(float result[4], const float quat1[4], const float quat2[4], const float t) { - result[0] = quat1[0] + t * quat2[0]; - result[1] = quat1[1] + t * quat2[1]; - result[2] = quat1[2] + t * quat2[2]; - result[3] = quat1[3] + t * quat2[3]; + result[0] = quat1[0] + t * quat2[0]; + result[1] = quat1[1] + t * quat2[1]; + result[2] = quat1[2] + t * quat2[2]; + result[3] = quat1[3] + t * quat2[3]; } /* same as tri_to_quat() but takes pre-computed normal from the triangle * used for ngons when we know their normal */ -void tri_to_quat_ex(float quat[4], const float v1[3], const float v2[3], const float v3[3], - const float no_orig[3]) +void tri_to_quat_ex( + float quat[4], const float v1[3], const float v2[3], const float v3[3], const float no_orig[3]) { - /* imaginary x-axis, y-axis triangle is being rotated */ - float vec[3], q1[4], q2[4], n[3], si, co, angle, mat[3][3], imat[3][3]; + /* imaginary x-axis, y-axis triangle is being rotated */ + float vec[3], q1[4], q2[4], n[3], si, co, angle, mat[3][3], imat[3][3]; - /* move z-axis to face-normal */ + /* move z-axis to face-normal */ #if 0 - normal_tri_v3(vec, v1, v2, v3); + normal_tri_v3(vec, v1, v2, v3); #else - copy_v3_v3(vec, no_orig); - (void)v3; + copy_v3_v3(vec, no_orig); + (void)v3; #endif - n[0] = vec[1]; - n[1] = -vec[0]; - n[2] = 0.0f; - normalize_v3(n); + n[0] = vec[1]; + n[1] = -vec[0]; + n[2] = 0.0f; + normalize_v3(n); - if (n[0] == 0.0f && n[1] == 0.0f) { - n[0] = 1.0f; - } + if (n[0] == 0.0f && n[1] == 0.0f) { + n[0] = 1.0f; + } - angle = -0.5f * saacos(vec[2]); - co = cosf(angle); - si = sinf(angle); - q1[0] = co; - q1[1] = n[0] * si; - q1[2] = n[1] * si; - q1[3] = 0.0f; + angle = -0.5f * saacos(vec[2]); + co = cosf(angle); + si = sinf(angle); + q1[0] = co; + q1[1] = n[0] * si; + q1[2] = n[1] * si; + q1[3] = 0.0f; - /* rotate back line v1-v2 */ - quat_to_mat3(mat, q1); - invert_m3_m3(imat, mat); - sub_v3_v3v3(vec, v2, v1); - mul_m3_v3(imat, vec); + /* rotate back line v1-v2 */ + quat_to_mat3(mat, q1); + invert_m3_m3(imat, mat); + sub_v3_v3v3(vec, v2, v1); + mul_m3_v3(imat, vec); - /* what angle has this line with x-axis? */ - vec[2] = 0.0f; - normalize_v3(vec); + /* what angle has this line with x-axis? */ + vec[2] = 0.0f; + normalize_v3(vec); - angle = 0.5f * atan2f(vec[1], vec[0]); - co = cosf(angle); - si = sinf(angle); - q2[0] = co; - q2[1] = 0.0f; - q2[2] = 0.0f; - q2[3] = si; + angle = 0.5f * atan2f(vec[1], vec[0]); + co = cosf(angle); + si = sinf(angle); + q2[0] = co; + q2[1] = 0.0f; + q2[2] = 0.0f; + q2[3] = si; - mul_qt_qtqt(quat, q1, q2); + mul_qt_qtqt(quat, q1, q2); } /** @@ -903,91 +917,94 @@ void tri_to_quat_ex(float quat[4], const float v1[3], const float v2[3], const f */ float tri_to_quat(float quat[4], const float v1[3], const float v2[3], const float v3[3]) { - float vec[3]; - const float len = normal_tri_v3(vec, v1, v2, v3); + float vec[3]; + const float len = normal_tri_v3(vec, v1, v2, v3); - tri_to_quat_ex(quat, v1, v2, v3, vec); - return len; + tri_to_quat_ex(quat, v1, v2, v3, vec); + return len; } void print_qt(const char *str, const float q[4]) { - printf("%s: %.3f %.3f %.3f %.3f\n", str, q[0], q[1], q[2], q[3]); + printf("%s: %.3f %.3f %.3f %.3f\n", str, q[0], q[1], q[2], q[3]); } /******************************** Axis Angle *********************************/ void axis_angle_normalized_to_quat(float q[4], const float axis[3], const float angle) { - const float phi = 0.5f * angle; - const float si = sinf(phi); - const float co = cosf(phi); - BLI_ASSERT_UNIT_V3(axis); - q[0] = co; - mul_v3_v3fl(q + 1, axis, si); + const float phi = 0.5f * angle; + const float si = sinf(phi); + const float co = cosf(phi); + BLI_ASSERT_UNIT_V3(axis); + q[0] = co; + mul_v3_v3fl(q + 1, axis, si); } void axis_angle_to_quat(float q[4], const float axis[3], const float angle) { - float nor[3]; + float nor[3]; - if (LIKELY(normalize_v3_v3(nor, axis) != 0.0f)) { - axis_angle_normalized_to_quat(q, nor, angle); - } - else { - unit_qt(q); - } + if (LIKELY(normalize_v3_v3(nor, axis) != 0.0f)) { + axis_angle_normalized_to_quat(q, nor, angle); + } + else { + unit_qt(q); + } } /* Quaternions to Axis Angle */ void quat_to_axis_angle(float axis[3], float *angle, const float q[4]) { - float ha, si; + float ha, si; #ifdef DEBUG - if (!((ha = dot_qtqt(q, q)) == 0.0f || (fabsf(ha - 1.0f) < (float)QUAT_EPSILON))) { - fprintf(stderr, "Warning! quat_to_axis_angle() called with non-normalized: size %.8f *** report a bug ***\n", ha); - } + if (!((ha = dot_qtqt(q, q)) == 0.0f || (fabsf(ha - 1.0f) < (float)QUAT_EPSILON))) { + fprintf(stderr, + "Warning! quat_to_axis_angle() called with non-normalized: size %.8f *** report a bug " + "***\n", + ha); + } #endif - /* calculate angle/2, and sin(angle/2) */ - ha = acosf(q[0]); - si = sinf(ha); + /* calculate angle/2, and sin(angle/2) */ + ha = acosf(q[0]); + si = sinf(ha); - /* from half-angle to angle */ - *angle = ha * 2; + /* from half-angle to angle */ + *angle = ha * 2; - /* prevent division by zero for axis conversion */ - if (fabsf(si) < 0.0005f) { - si = 1.0f; - } + /* prevent division by zero for axis conversion */ + if (fabsf(si) < 0.0005f) { + si = 1.0f; + } - axis[0] = q[1] / si; - axis[1] = q[2] / si; - axis[2] = q[3] / si; - if (is_zero_v3(axis)) { - axis[1] = 1.0f; - } + axis[0] = q[1] / si; + axis[1] = q[2] / si; + axis[2] = q[3] / si; + if (is_zero_v3(axis)) { + axis[1] = 1.0f; + } } /* Axis Angle to Euler Rotation */ void axis_angle_to_eulO(float eul[3], const short order, const float axis[3], const float angle) { - float q[4]; + float q[4]; - /* use quaternions as intermediate representation for now... */ - axis_angle_to_quat(q, axis, angle); - quat_to_eulO(eul, order, q); + /* use quaternions as intermediate representation for now... */ + axis_angle_to_quat(q, axis, angle); + quat_to_eulO(eul, order, q); } /* Euler Rotation to Axis Angle */ void eulO_to_axis_angle(float axis[3], float *angle, const float eul[3], const short order) { - float q[4]; + float q[4]; - /* use quaternions as intermediate representation for now... */ - eulO_to_quat(q, eul, order); - quat_to_axis_angle(axis, angle, q); + /* use quaternions as intermediate representation for now... */ + eulO_to_quat(q, eul, order); + quat_to_axis_angle(axis, angle, q); } /** @@ -999,223 +1016,224 @@ void eulO_to_axis_angle(float axis[3], float *angle, const float eul[3], const s * \param angle_sin: sin(angle) * \param angle_cos: cos(angle) */ -void axis_angle_normalized_to_mat3_ex(float mat[3][3], const float axis[3], - const float angle_sin, const float angle_cos) +void axis_angle_normalized_to_mat3_ex(float mat[3][3], + const float axis[3], + const float angle_sin, + const float angle_cos) { - float nsi[3], ico; - float n_00, n_01, n_11, n_02, n_12, n_22; + float nsi[3], ico; + float n_00, n_01, n_11, n_02, n_12, n_22; - BLI_ASSERT_UNIT_V3(axis); + BLI_ASSERT_UNIT_V3(axis); - /* now convert this to a 3x3 matrix */ + /* now convert this to a 3x3 matrix */ - ico = (1.0f - angle_cos); - nsi[0] = axis[0] * angle_sin; - nsi[1] = axis[1] * angle_sin; - nsi[2] = axis[2] * angle_sin; + ico = (1.0f - angle_cos); + nsi[0] = axis[0] * angle_sin; + nsi[1] = axis[1] * angle_sin; + nsi[2] = axis[2] * angle_sin; - n_00 = (axis[0] * axis[0]) * ico; - n_01 = (axis[0] * axis[1]) * ico; - n_11 = (axis[1] * axis[1]) * ico; - n_02 = (axis[0] * axis[2]) * ico; - n_12 = (axis[1] * axis[2]) * ico; - n_22 = (axis[2] * axis[2]) * ico; + n_00 = (axis[0] * axis[0]) * ico; + n_01 = (axis[0] * axis[1]) * ico; + n_11 = (axis[1] * axis[1]) * ico; + n_02 = (axis[0] * axis[2]) * ico; + n_12 = (axis[1] * axis[2]) * ico; + n_22 = (axis[2] * axis[2]) * ico; - mat[0][0] = n_00 + 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][1] = n_11 + angle_cos; - mat[1][2] = n_12 + nsi[0]; - mat[2][0] = n_02 + nsi[1]; - mat[2][1] = n_12 - nsi[0]; - mat[2][2] = n_22 + angle_cos; + mat[0][0] = n_00 + 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][1] = n_11 + angle_cos; + mat[1][2] = n_12 + nsi[0]; + mat[2][0] = n_02 + nsi[1]; + mat[2][1] = n_12 - nsi[0]; + mat[2][2] = n_22 + angle_cos; } void axis_angle_normalized_to_mat3(float mat[3][3], const float axis[3], const float angle) { - axis_angle_normalized_to_mat3_ex(mat, axis, sinf(angle), cosf(angle)); + axis_angle_normalized_to_mat3_ex(mat, axis, sinf(angle), cosf(angle)); } - /* axis angle to 3x3 matrix - safer version (normalization of axis performed) */ void axis_angle_to_mat3(float mat[3][3], const float axis[3], const float angle) { - float nor[3]; + float nor[3]; - /* normalize the axis first (to remove unwanted scaling) */ - if (normalize_v3_v3(nor, axis) == 0.0f) { - unit_m3(mat); - return; - } + /* normalize the axis first (to remove unwanted scaling) */ + if (normalize_v3_v3(nor, axis) == 0.0f) { + unit_m3(mat); + return; + } - axis_angle_normalized_to_mat3(mat, nor, angle); + axis_angle_normalized_to_mat3(mat, nor, angle); } /* axis angle to 4x4 matrix - safer version (normalization of axis performed) */ void axis_angle_to_mat4(float mat[4][4], const float axis[3], const float angle) { - float tmat[3][3]; + float tmat[3][3]; - axis_angle_to_mat3(tmat, axis, angle); - unit_m4(mat); - copy_m4_m3(mat, tmat); + axis_angle_to_mat3(tmat, axis, angle); + unit_m4(mat); + copy_m4_m3(mat, tmat); } /* 3x3 matrix to axis angle */ void mat3_normalized_to_axis_angle(float axis[3], float *angle, const float mat[3][3]) { - float q[4]; + float q[4]; - /* use quaternions as intermediate representation */ - /* TODO: it would be nicer to go straight there... */ - mat3_normalized_to_quat(q, mat); - quat_to_axis_angle(axis, angle, q); + /* use quaternions as intermediate representation */ + /* TODO: it would be nicer to go straight there... */ + mat3_normalized_to_quat(q, mat); + quat_to_axis_angle(axis, angle, q); } void mat3_to_axis_angle(float axis[3], float *angle, const float mat[3][3]) { - float q[4]; + float q[4]; - /* use quaternions as intermediate representation */ - /* TODO: it would be nicer to go straight there... */ - mat3_to_quat(q, mat); - quat_to_axis_angle(axis, angle, q); + /* use quaternions as intermediate representation */ + /* TODO: it would be nicer to go straight there... */ + mat3_to_quat(q, mat); + quat_to_axis_angle(axis, angle, q); } /* 4x4 matrix to axis angle */ void mat4_normalized_to_axis_angle(float axis[3], float *angle, const float mat[4][4]) { - float q[4]; + float q[4]; - /* use quaternions as intermediate representation */ - /* TODO: it would be nicer to go straight there... */ - mat4_normalized_to_quat(q, mat); - quat_to_axis_angle(axis, angle, q); + /* use quaternions as intermediate representation */ + /* TODO: it would be nicer to go straight there... */ + mat4_normalized_to_quat(q, mat); + quat_to_axis_angle(axis, angle, q); } /* 4x4 matrix to axis angle */ void mat4_to_axis_angle(float axis[3], float *angle, const float mat[4][4]) { - float q[4]; + float q[4]; - /* use quaternions as intermediate representation */ - /* TODO: it would be nicer to go straight there... */ - mat4_to_quat(q, mat); - quat_to_axis_angle(axis, angle, q); + /* use quaternions as intermediate representation */ + /* TODO: it would be nicer to go straight there... */ + mat4_to_quat(q, mat); + quat_to_axis_angle(axis, angle, q); } void axis_angle_to_mat4_single(float mat[4][4], const char axis, const float angle) { - float mat3[3][3]; - axis_angle_to_mat3_single(mat3, axis, angle); - copy_m4_m3(mat, mat3); + float mat3[3][3]; + axis_angle_to_mat3_single(mat3, axis, angle); + copy_m4_m3(mat, mat3); } /* rotation matrix from a single axis */ void axis_angle_to_mat3_single(float mat[3][3], const char axis, const float angle) { - const float angle_cos = cosf(angle); - const float angle_sin = sinf(angle); - - switch (axis) { - case 'X': /* rotation around X */ - mat[0][0] = 1.0f; - mat[0][1] = 0.0f; - mat[0][2] = 0.0f; - mat[1][0] = 0.0f; - mat[1][1] = angle_cos; - mat[1][2] = angle_sin; - mat[2][0] = 0.0f; - mat[2][1] = -angle_sin; - mat[2][2] = angle_cos; - break; - case 'Y': /* rotation around Y */ - mat[0][0] = angle_cos; - mat[0][1] = 0.0f; - mat[0][2] = -angle_sin; - mat[1][0] = 0.0f; - mat[1][1] = 1.0f; - mat[1][2] = 0.0f; - mat[2][0] = angle_sin; - mat[2][1] = 0.0f; - mat[2][2] = angle_cos; - break; - case 'Z': /* rotation around Z */ - mat[0][0] = angle_cos; - mat[0][1] = angle_sin; - mat[0][2] = 0.0f; - mat[1][0] = -angle_sin; - mat[1][1] = angle_cos; - mat[1][2] = 0.0f; - mat[2][0] = 0.0f; - mat[2][1] = 0.0f; - mat[2][2] = 1.0f; - break; - default: - BLI_assert(0); - break; - } + const float angle_cos = cosf(angle); + const float angle_sin = sinf(angle); + + switch (axis) { + case 'X': /* rotation around X */ + mat[0][0] = 1.0f; + mat[0][1] = 0.0f; + mat[0][2] = 0.0f; + mat[1][0] = 0.0f; + mat[1][1] = angle_cos; + mat[1][2] = angle_sin; + mat[2][0] = 0.0f; + mat[2][1] = -angle_sin; + mat[2][2] = angle_cos; + break; + case 'Y': /* rotation around Y */ + mat[0][0] = angle_cos; + mat[0][1] = 0.0f; + mat[0][2] = -angle_sin; + mat[1][0] = 0.0f; + mat[1][1] = 1.0f; + mat[1][2] = 0.0f; + mat[2][0] = angle_sin; + mat[2][1] = 0.0f; + mat[2][2] = angle_cos; + break; + case 'Z': /* rotation around Z */ + mat[0][0] = angle_cos; + mat[0][1] = angle_sin; + mat[0][2] = 0.0f; + mat[1][0] = -angle_sin; + mat[1][1] = angle_cos; + mat[1][2] = 0.0f; + mat[2][0] = 0.0f; + mat[2][1] = 0.0f; + mat[2][2] = 1.0f; + break; + default: + BLI_assert(0); + break; + } } void angle_to_mat2(float mat[2][2], const float angle) { - const float angle_cos = cosf(angle); - const float angle_sin = sinf(angle); + const float angle_cos = cosf(angle); + const float angle_sin = sinf(angle); - /* 2D rotation matrix */ - mat[0][0] = angle_cos; - mat[0][1] = angle_sin; - mat[1][0] = -angle_sin; - mat[1][1] = angle_cos; + /* 2D rotation matrix */ + mat[0][0] = angle_cos; + mat[0][1] = angle_sin; + mat[1][0] = -angle_sin; + mat[1][1] = angle_cos; } void axis_angle_to_quat_single(float q[4], const char axis, const float angle) { - const float angle_half = angle * 0.5f; - const float angle_cos = cosf(angle_half); - const float angle_sin = sinf(angle_half); - const int axis_index = (axis - 'X'); + const float angle_half = angle * 0.5f; + const float angle_cos = cosf(angle_half); + const float angle_sin = sinf(angle_half); + const int axis_index = (axis - 'X'); - assert(axis >= 'X' && axis <= 'Z'); + assert(axis >= 'X' && axis <= 'Z'); - q[0] = angle_cos; - zero_v3(q + 1); - q[axis_index + 1] = angle_sin; + q[0] = angle_cos; + zero_v3(q + 1); + q[axis_index + 1] = angle_sin; } /****************************** Exponential Map ******************************/ void quat_normalized_to_expmap(float expmap[3], const float q[4]) { - float angle; - BLI_ASSERT_UNIT_QUAT(q); + float angle; + BLI_ASSERT_UNIT_QUAT(q); - /* Obtain axis/angle representation. */ - quat_to_axis_angle(expmap, &angle, q); + /* Obtain axis/angle representation. */ + quat_to_axis_angle(expmap, &angle, q); - /* Convert to exponential map. */ - mul_v3_fl(expmap, angle); + /* Convert to exponential map. */ + mul_v3_fl(expmap, angle); } void quat_to_expmap(float expmap[3], const float q[4]) { - float q_no[4]; - normalize_qt_qt(q_no, q); - quat_normalized_to_expmap(expmap, q_no); + float q_no[4]; + normalize_qt_qt(q_no, q); + quat_normalized_to_expmap(expmap, q_no); } void expmap_to_quat(float r[4], const float expmap[3]) { - float axis[3]; - float angle; + float axis[3]; + float angle; - /* Obtain axis/angle representation. */ - if (LIKELY((angle = normalize_v3_v3(axis, expmap)) != 0.0f)) { - axis_angle_normalized_to_quat(r, axis, angle_wrap_rad(angle)); - } - else { - unit_qt(r); - } + /* Obtain axis/angle representation. */ + if (LIKELY((angle = normalize_v3_v3(axis, expmap)) != 0.0f)) { + axis_angle_normalized_to_quat(r, axis, angle_wrap_rad(angle)); + } + else { + unit_qt(r); + } } /******************************** XYZ Eulers *********************************/ @@ -1223,60 +1241,58 @@ void expmap_to_quat(float r[4], const float expmap[3]) /* XYZ order */ void eul_to_mat3(float mat[3][3], const float eul[3]) { - double ci, cj, ch, si, sj, sh, cc, cs, sc, ss; - - ci = cos(eul[0]); - cj = cos(eul[1]); - ch = cos(eul[2]); - si = sin(eul[0]); - sj = sin(eul[1]); - sh = sin(eul[2]); - cc = ci * ch; - cs = ci * sh; - sc = si * ch; - ss = si * sh; - - mat[0][0] = (float)(cj * ch); - mat[1][0] = (float)(sj * sc - cs); - mat[2][0] = (float)(sj * cc + ss); - mat[0][1] = (float)(cj * sh); - mat[1][1] = (float)(sj * ss + cc); - mat[2][1] = (float)(sj * cs - sc); - mat[0][2] = (float)-sj; - mat[1][2] = (float)(cj * si); - mat[2][2] = (float)(cj * ci); - + double ci, cj, ch, si, sj, sh, cc, cs, sc, ss; + + ci = cos(eul[0]); + cj = cos(eul[1]); + ch = cos(eul[2]); + si = sin(eul[0]); + sj = sin(eul[1]); + sh = sin(eul[2]); + cc = ci * ch; + cs = ci * sh; + sc = si * ch; + ss = si * sh; + + mat[0][0] = (float)(cj * ch); + mat[1][0] = (float)(sj * sc - cs); + mat[2][0] = (float)(sj * cc + ss); + mat[0][1] = (float)(cj * sh); + mat[1][1] = (float)(sj * ss + cc); + mat[2][1] = (float)(sj * cs - sc); + mat[0][2] = (float)-sj; + mat[1][2] = (float)(cj * si); + mat[2][2] = (float)(cj * ci); } /* XYZ order */ void eul_to_mat4(float mat[4][4], const float eul[3]) { - double ci, cj, ch, si, sj, sh, cc, cs, sc, ss; - - ci = cos(eul[0]); - cj = cos(eul[1]); - ch = cos(eul[2]); - si = sin(eul[0]); - sj = sin(eul[1]); - sh = sin(eul[2]); - cc = ci * ch; - cs = ci * sh; - sc = si * ch; - ss = si * sh; - - mat[0][0] = (float)(cj * ch); - mat[1][0] = (float)(sj * sc - cs); - mat[2][0] = (float)(sj * cc + ss); - mat[0][1] = (float)(cj * sh); - mat[1][1] = (float)(sj * ss + cc); - mat[2][1] = (float)(sj * cs - sc); - mat[0][2] = (float)-sj; - mat[1][2] = (float)(cj * si); - mat[2][2] = (float)(cj * ci); - - - mat[3][0] = mat[3][1] = mat[3][2] = mat[0][3] = mat[1][3] = mat[2][3] = 0.0f; - mat[3][3] = 1.0f; + double ci, cj, ch, si, sj, sh, cc, cs, sc, ss; + + ci = cos(eul[0]); + cj = cos(eul[1]); + ch = cos(eul[2]); + si = sin(eul[0]); + sj = sin(eul[1]); + sh = sin(eul[2]); + cc = ci * ch; + cs = ci * sh; + sc = si * ch; + ss = si * sh; + + mat[0][0] = (float)(cj * ch); + mat[1][0] = (float)(sj * sc - cs); + mat[2][0] = (float)(sj * cc + ss); + mat[0][1] = (float)(cj * sh); + mat[1][1] = (float)(sj * ss + cc); + mat[2][1] = (float)(sj * cs - sc); + mat[0][2] = (float)-sj; + mat[1][2] = (float)(cj * si); + mat[2][2] = (float)(cj * ci); + + mat[3][0] = mat[3][1] = mat[3][2] = mat[0][3] = mat[1][3] = mat[2][3] = 0.0f; + mat[3][3] = 1.0f; } /* returns two euler calculation methods, so we can pick the best */ @@ -1284,162 +1300,174 @@ void eul_to_mat4(float mat[4][4], const float eul[3]) /* XYZ order */ static void mat3_normalized_to_eul2(const float mat[3][3], float eul1[3], float eul2[3]) { - const float cy = hypotf(mat[0][0], mat[0][1]); + const float cy = hypotf(mat[0][0], mat[0][1]); - BLI_ASSERT_UNIT_M3(mat); + BLI_ASSERT_UNIT_M3(mat); - if (cy > 16.0f * FLT_EPSILON) { + if (cy > 16.0f * FLT_EPSILON) { - eul1[0] = atan2f(mat[1][2], mat[2][2]); - eul1[1] = atan2f(-mat[0][2], cy); - eul1[2] = atan2f(mat[0][1], mat[0][0]); + eul1[0] = atan2f(mat[1][2], mat[2][2]); + eul1[1] = atan2f(-mat[0][2], cy); + eul1[2] = atan2f(mat[0][1], mat[0][0]); - eul2[0] = atan2f(-mat[1][2], -mat[2][2]); - eul2[1] = atan2f(-mat[0][2], -cy); - eul2[2] = atan2f(-mat[0][1], -mat[0][0]); + eul2[0] = atan2f(-mat[1][2], -mat[2][2]); + eul2[1] = atan2f(-mat[0][2], -cy); + eul2[2] = atan2f(-mat[0][1], -mat[0][0]); + } + else { + eul1[0] = atan2f(-mat[2][1], mat[1][1]); + eul1[1] = atan2f(-mat[0][2], cy); + eul1[2] = 0.0f; - } - else { - eul1[0] = atan2f(-mat[2][1], mat[1][1]); - eul1[1] = atan2f(-mat[0][2], cy); - eul1[2] = 0.0f; - - copy_v3_v3(eul2, eul1); - } + copy_v3_v3(eul2, eul1); + } } /* XYZ order */ void mat3_normalized_to_eul(float eul[3], const float mat[3][3]) { - float eul1[3], eul2[3]; + float eul1[3], eul2[3]; - mat3_normalized_to_eul2(mat, eul1, eul2); + mat3_normalized_to_eul2(mat, eul1, eul2); - /* return best, which is just the one with lowest values it in */ - if (fabsf(eul1[0]) + fabsf(eul1[1]) + fabsf(eul1[2]) > fabsf(eul2[0]) + fabsf(eul2[1]) + fabsf(eul2[2])) { - copy_v3_v3(eul, eul2); - } - else { - copy_v3_v3(eul, eul1); - } + /* return best, which is just the one with lowest values it in */ + if (fabsf(eul1[0]) + fabsf(eul1[1]) + fabsf(eul1[2]) > + fabsf(eul2[0]) + fabsf(eul2[1]) + fabsf(eul2[2])) { + copy_v3_v3(eul, eul2); + } + else { + copy_v3_v3(eul, eul1); + } } void mat3_to_eul(float eul[3], const float mat[3][3]) { - float unit_mat[3][3]; - normalize_m3_m3(unit_mat, mat); - mat3_normalized_to_eul(eul, unit_mat); + float unit_mat[3][3]; + normalize_m3_m3(unit_mat, mat); + mat3_normalized_to_eul(eul, unit_mat); } /* XYZ order */ void mat4_normalized_to_eul(float eul[3], const float m[4][4]) { - float mat3[3][3]; - copy_m3_m4(mat3, m); - mat3_normalized_to_eul(eul, mat3); + float mat3[3][3]; + copy_m3_m4(mat3, m); + mat3_normalized_to_eul(eul, mat3); } void mat4_to_eul(float eul[3], const float m[4][4]) { - float mat3[3][3]; - copy_m3_m4(mat3, m); - mat3_to_eul(eul, mat3); + float mat3[3][3]; + copy_m3_m4(mat3, m); + mat3_to_eul(eul, mat3); } /* XYZ order */ void quat_to_eul(float eul[3], const float quat[4]) { - float unit_mat[3][3]; - quat_to_mat3(unit_mat, quat); - mat3_normalized_to_eul(eul, unit_mat); + float unit_mat[3][3]; + quat_to_mat3(unit_mat, quat); + mat3_normalized_to_eul(eul, unit_mat); } /* XYZ order */ void eul_to_quat(float quat[4], const float eul[3]) { - float ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss; + float ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss; - ti = eul[0] * 0.5f; - tj = eul[1] * 0.5f; - th = eul[2] * 0.5f; - ci = cosf(ti); - cj = cosf(tj); - ch = cosf(th); - si = sinf(ti); - sj = sinf(tj); - sh = sinf(th); - cc = ci * ch; - cs = ci * sh; - sc = si * ch; - ss = si * sh; + ti = eul[0] * 0.5f; + tj = eul[1] * 0.5f; + th = eul[2] * 0.5f; + ci = cosf(ti); + cj = cosf(tj); + ch = cosf(th); + si = sinf(ti); + sj = sinf(tj); + sh = sinf(th); + cc = ci * ch; + cs = ci * sh; + sc = si * ch; + ss = si * sh; - quat[0] = cj * cc + sj * ss; - quat[1] = cj * sc - sj * cs; - quat[2] = cj * ss + sj * cc; - quat[3] = cj * cs - sj * sc; + quat[0] = cj * cc + sj * ss; + quat[1] = cj * sc - sj * cs; + quat[2] = cj * ss + sj * cc; + quat[3] = cj * cs - sj * sc; } /* XYZ order */ void rotate_eul(float beul[3], const char axis, const float ang) { - float eul[3], mat1[3][3], mat2[3][3], totmat[3][3]; + float eul[3], mat1[3][3], mat2[3][3], totmat[3][3]; - assert(axis >= 'X' && axis <= 'Z'); + assert(axis >= 'X' && axis <= 'Z'); - eul[0] = eul[1] = eul[2] = 0.0f; - if (axis == 'X') { - eul[0] = ang; - } - else if (axis == 'Y') { - eul[1] = ang; - } - else { - eul[2] = ang; - } + eul[0] = eul[1] = eul[2] = 0.0f; + if (axis == 'X') { + eul[0] = ang; + } + else if (axis == 'Y') { + eul[1] = ang; + } + else { + eul[2] = ang; + } - eul_to_mat3(mat1, eul); - eul_to_mat3(mat2, beul); + eul_to_mat3(mat1, eul); + eul_to_mat3(mat2, beul); - mul_m3_m3m3(totmat, mat2, mat1); + mul_m3_m3m3(totmat, mat2, mat1); - mat3_to_eul(beul, totmat); + mat3_to_eul(beul, totmat); } /* order independent! */ void compatible_eul(float eul[3], const float oldrot[3]) { - /* we could use M_PI as pi_thresh: which is correct but 5.1 gives better results. - * Checked with baking actions to fcurves - campbell */ - const float pi_thresh = (5.1f); - const float pi_x2 = (2.0f * (float)M_PI); - - float deul[3]; - unsigned int i; - - /* correct differences of about 360 degrees first */ - for (i = 0; i < 3; i++) { - deul[i] = eul[i] - oldrot[i]; - if (deul[i] > pi_thresh) { - eul[i] -= floorf(( deul[i] / pi_x2) + 0.5f) * pi_x2; - deul[i] = eul[i] - oldrot[i]; - } - else if (deul[i] < -pi_thresh) { - eul[i] += floorf((-deul[i] / pi_x2) + 0.5f) * pi_x2; - deul[i] = eul[i] - oldrot[i]; - } - } - - /* is 1 of the axis rotations larger than 180 degrees and the other small? NO ELSE IF!! */ - if (fabsf(deul[0]) > 3.2f && fabsf(deul[1]) < 1.6f && fabsf(deul[2]) < 1.6f) { - if (deul[0] > 0.0f) { eul[0] -= pi_x2; } - else { eul[0] += pi_x2; } - } - if (fabsf(deul[1]) > 3.2f && fabsf(deul[2]) < 1.6f && fabsf(deul[0]) < 1.6f) { - if (deul[1] > 0.0f) { eul[1] -= pi_x2; } - else { eul[1] += pi_x2; } - } - if (fabsf(deul[2]) > 3.2f && fabsf(deul[0]) < 1.6f && fabsf(deul[1]) < 1.6f) { - if (deul[2] > 0.0f) { eul[2] -= pi_x2; } - else { eul[2] += pi_x2; } - } + /* we could use M_PI as pi_thresh: which is correct but 5.1 gives better results. + * Checked with baking actions to fcurves - campbell */ + const float pi_thresh = (5.1f); + const float pi_x2 = (2.0f * (float)M_PI); + + float deul[3]; + unsigned int i; + + /* correct differences of about 360 degrees first */ + for (i = 0; i < 3; i++) { + deul[i] = eul[i] - oldrot[i]; + if (deul[i] > pi_thresh) { + eul[i] -= floorf((deul[i] / pi_x2) + 0.5f) * pi_x2; + deul[i] = eul[i] - oldrot[i]; + } + else if (deul[i] < -pi_thresh) { + eul[i] += floorf((-deul[i] / pi_x2) + 0.5f) * pi_x2; + deul[i] = eul[i] - oldrot[i]; + } + } + + /* is 1 of the axis rotations larger than 180 degrees and the other small? NO ELSE IF!! */ + if (fabsf(deul[0]) > 3.2f && fabsf(deul[1]) < 1.6f && fabsf(deul[2]) < 1.6f) { + if (deul[0] > 0.0f) { + eul[0] -= pi_x2; + } + else { + eul[0] += pi_x2; + } + } + if (fabsf(deul[1]) > 3.2f && fabsf(deul[2]) < 1.6f && fabsf(deul[0]) < 1.6f) { + if (deul[1] > 0.0f) { + eul[1] -= pi_x2; + } + else { + eul[1] += pi_x2; + } + } + if (fabsf(deul[2]) > 3.2f && fabsf(deul[0]) < 1.6f && fabsf(deul[1]) < 1.6f) { + if (deul[2] > 0.0f) { + eul[2] -= pi_x2; + } + else { + eul[2] += pi_x2; + } + } } /* uses 2 methods to retrieve eulers, and picks the closest */ @@ -1447,37 +1475,37 @@ void compatible_eul(float eul[3], const float oldrot[3]) /* XYZ order */ void mat3_normalized_to_compatible_eul(float eul[3], const float oldrot[3], float mat[3][3]) { - float eul1[3], eul2[3]; - float d1, d2; + float eul1[3], eul2[3]; + float d1, d2; - mat3_normalized_to_eul2(mat, eul1, eul2); + mat3_normalized_to_eul2(mat, eul1, eul2); - compatible_eul(eul1, oldrot); - compatible_eul(eul2, oldrot); + compatible_eul(eul1, oldrot); + compatible_eul(eul2, oldrot); - d1 = fabsf(eul1[0] - oldrot[0]) + fabsf(eul1[1] - oldrot[1]) + fabsf(eul1[2] - oldrot[2]); - d2 = fabsf(eul2[0] - oldrot[0]) + fabsf(eul2[1] - oldrot[1]) + fabsf(eul2[2] - oldrot[2]); + d1 = fabsf(eul1[0] - oldrot[0]) + fabsf(eul1[1] - oldrot[1]) + fabsf(eul1[2] - oldrot[2]); + d2 = fabsf(eul2[0] - oldrot[0]) + fabsf(eul2[1] - oldrot[1]) + fabsf(eul2[2] - oldrot[2]); - /* return best, which is just the one with lowest difference */ - if (d1 > d2) { - copy_v3_v3(eul, eul2); - } - else { - copy_v3_v3(eul, eul1); - } + /* return best, which is just the one with lowest difference */ + if (d1 > d2) { + copy_v3_v3(eul, eul2); + } + else { + copy_v3_v3(eul, eul1); + } } void mat3_to_compatible_eul(float eul[3], const float oldrot[3], float mat[3][3]) { - float unit_mat[3][3]; - normalize_m3_m3(unit_mat, mat); - mat3_normalized_to_compatible_eul(eul, oldrot, unit_mat); + float unit_mat[3][3]; + normalize_m3_m3(unit_mat, mat); + mat3_normalized_to_compatible_eul(eul, oldrot, unit_mat); } void quat_to_compatible_eul(float eul[3], const float oldrot[3], const float quat[4]) { - float unit_mat[3][3]; - quat_to_mat3(unit_mat, quat); - mat3_normalized_to_compatible_eul(eul, oldrot, unit_mat); + float unit_mat[3][3]; + quat_to_mat3(unit_mat, quat); + mat3_normalized_to_compatible_eul(eul, oldrot, unit_mat); } /************************** Arbitrary Order Eulers ***************************/ @@ -1493,21 +1521,21 @@ void quat_to_compatible_eul(float eul[3], const float oldrot[3], const float qua /* Type for rotation order info - see wiki for derivation details */ typedef struct RotOrderInfo { - short axis[3]; - short parity; /* parity of axis permutation (even=0, odd=1) - 'n' in original code */ + short axis[3]; + short parity; /* parity of axis permutation (even=0, odd=1) - 'n' in original code */ } RotOrderInfo; /* Array of info for Rotation Order calculations * WARNING: must be kept in same order as eEulerRotationOrders */ static const RotOrderInfo rotOrders[] = { - /* i, j, k, n */ - {{0, 1, 2}, 0}, /* XYZ */ - {{0, 2, 1}, 1}, /* XZY */ - {{1, 0, 2}, 1}, /* YXZ */ - {{1, 2, 0}, 0}, /* YZX */ - {{2, 0, 1}, 0}, /* ZXY */ - {{2, 1, 0}, 1} /* ZYX */ + /* i, j, k, n */ + {{0, 1, 2}, 0}, /* XYZ */ + {{0, 2, 1}, 1}, /* XZY */ + {{1, 0, 2}, 1}, /* YXZ */ + {{1, 2, 0}, 0}, /* YZX */ + {{2, 0, 1}, 0}, /* ZXY */ + {{2, 1, 0}, 1} /* ZYX */ }; /* Get relevant pointer to rotation order set from the array @@ -1516,254 +1544,266 @@ static const RotOrderInfo rotOrders[] = { */ static const RotOrderInfo *get_rotation_order_info(const short order) { - assert(order >= 0 && order <= 6); - if (order < 1) { - return &rotOrders[0]; - } - else if (order < 6) { - return &rotOrders[order - 1]; - } - else { - return &rotOrders[5]; - } + assert(order >= 0 && order <= 6); + if (order < 1) { + return &rotOrders[0]; + } + else if (order < 6) { + return &rotOrders[order - 1]; + } + else { + return &rotOrders[5]; + } } /* Construct quaternion from Euler angles (in radians). */ void eulO_to_quat(float q[4], const float e[3], const short order) { - const RotOrderInfo *R = get_rotation_order_info(order); - short i = R->axis[0], j = R->axis[1], k = R->axis[2]; - double ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss; - double a[3]; + const RotOrderInfo *R = get_rotation_order_info(order); + short i = R->axis[0], j = R->axis[1], k = R->axis[2]; + double ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss; + double a[3]; - ti = e[i] * 0.5f; - tj = e[j] * (R->parity ? -0.5f : 0.5f); - th = e[k] * 0.5f; + ti = e[i] * 0.5f; + tj = e[j] * (R->parity ? -0.5f : 0.5f); + th = e[k] * 0.5f; - ci = cos(ti); - cj = cos(tj); - ch = cos(th); - si = sin(ti); - sj = sin(tj); - sh = sin(th); + ci = cos(ti); + cj = cos(tj); + ch = cos(th); + si = sin(ti); + sj = sin(tj); + sh = sin(th); - cc = ci * ch; - cs = ci * sh; - sc = si * ch; - ss = si * sh; + cc = ci * ch; + cs = ci * sh; + sc = si * ch; + ss = si * sh; - a[i] = cj * sc - sj * cs; - a[j] = cj * ss + sj * cc; - a[k] = cj * cs - sj * sc; + a[i] = cj * sc - sj * cs; + a[j] = cj * ss + sj * cc; + a[k] = cj * cs - sj * sc; - q[0] = (float)(cj * cc + sj * ss); - q[1] = (float)(a[0]); - q[2] = (float)(a[1]); - q[3] = (float)(a[2]); + q[0] = (float)(cj * cc + sj * ss); + q[1] = (float)(a[0]); + q[2] = (float)(a[1]); + q[3] = (float)(a[2]); - if (R->parity) { - q[j + 1] = -q[j + 1]; - } + if (R->parity) { + q[j + 1] = -q[j + 1]; + } } /* Convert quaternion to Euler angles (in radians). */ void quat_to_eulO(float e[3], short const order, const float q[4]) { - float unit_mat[3][3]; + float unit_mat[3][3]; - quat_to_mat3(unit_mat, q); - mat3_normalized_to_eulO(e, order, unit_mat); + quat_to_mat3(unit_mat, q); + mat3_normalized_to_eulO(e, order, unit_mat); } /* Construct 3x3 matrix from Euler angles (in radians). */ void eulO_to_mat3(float M[3][3], const float e[3], const short order) { - const RotOrderInfo *R = get_rotation_order_info(order); - short i = R->axis[0], j = R->axis[1], k = R->axis[2]; - double ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss; - - if (R->parity) { - ti = -e[i]; - tj = -e[j]; - th = -e[k]; - } - else { - ti = e[i]; - tj = e[j]; - th = e[k]; - } - - ci = cos(ti); - cj = cos(tj); - ch = cos(th); - si = sin(ti); - sj = sin(tj); - sh = sin(th); - - cc = ci * ch; - cs = ci * sh; - sc = si * ch; - ss = si * sh; - - M[i][i] = (float)(cj * ch); - M[j][i] = (float)(sj * sc - cs); - M[k][i] = (float)(sj * cc + ss); - M[i][j] = (float)(cj * sh); - M[j][j] = (float)(sj * ss + cc); - M[k][j] = (float)(sj * cs - sc); - M[i][k] = (float)(-sj); - M[j][k] = (float)(cj * si); - M[k][k] = (float)(cj * ci); + const RotOrderInfo *R = get_rotation_order_info(order); + short i = R->axis[0], j = R->axis[1], k = R->axis[2]; + double ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss; + + if (R->parity) { + ti = -e[i]; + tj = -e[j]; + th = -e[k]; + } + else { + ti = e[i]; + tj = e[j]; + th = e[k]; + } + + ci = cos(ti); + cj = cos(tj); + ch = cos(th); + si = sin(ti); + sj = sin(tj); + sh = sin(th); + + cc = ci * ch; + cs = ci * sh; + sc = si * ch; + ss = si * sh; + + M[i][i] = (float)(cj * ch); + M[j][i] = (float)(sj * sc - cs); + M[k][i] = (float)(sj * cc + ss); + M[i][j] = (float)(cj * sh); + M[j][j] = (float)(sj * ss + cc); + M[k][j] = (float)(sj * cs - sc); + M[i][k] = (float)(-sj); + M[j][k] = (float)(cj * si); + M[k][k] = (float)(cj * ci); } /* returns two euler calculation methods, so we can pick the best */ -static void mat3_normalized_to_eulo2(const float mat[3][3], float eul1[3], float eul2[3], const short order) +static void mat3_normalized_to_eulo2(const float mat[3][3], + float eul1[3], + float eul2[3], + const short order) { - const RotOrderInfo *R = get_rotation_order_info(order); - short i = R->axis[0], j = R->axis[1], k = R->axis[2]; - float cy; + const RotOrderInfo *R = get_rotation_order_info(order); + short i = R->axis[0], j = R->axis[1], k = R->axis[2]; + float cy; - BLI_ASSERT_UNIT_M3(mat); + BLI_ASSERT_UNIT_M3(mat); - cy = hypotf(mat[i][i], mat[i][j]); + cy = hypotf(mat[i][i], mat[i][j]); - if (cy > 16.0f * FLT_EPSILON) { - eul1[i] = atan2f(mat[j][k], mat[k][k]); - eul1[j] = atan2f(-mat[i][k], cy); - eul1[k] = atan2f(mat[i][j], mat[i][i]); + if (cy > 16.0f * FLT_EPSILON) { + eul1[i] = atan2f(mat[j][k], mat[k][k]); + eul1[j] = atan2f(-mat[i][k], cy); + eul1[k] = atan2f(mat[i][j], mat[i][i]); - eul2[i] = atan2f(-mat[j][k], -mat[k][k]); - eul2[j] = atan2f(-mat[i][k], -cy); - eul2[k] = atan2f(-mat[i][j], -mat[i][i]); - } - else { - eul1[i] = atan2f(-mat[k][j], mat[j][j]); - eul1[j] = atan2f(-mat[i][k], cy); - eul1[k] = 0; + eul2[i] = atan2f(-mat[j][k], -mat[k][k]); + eul2[j] = atan2f(-mat[i][k], -cy); + eul2[k] = atan2f(-mat[i][j], -mat[i][i]); + } + else { + eul1[i] = atan2f(-mat[k][j], mat[j][j]); + eul1[j] = atan2f(-mat[i][k], cy); + eul1[k] = 0; - copy_v3_v3(eul2, eul1); - } + copy_v3_v3(eul2, eul1); + } - if (R->parity) { - negate_v3(eul1); - negate_v3(eul2); - } + if (R->parity) { + negate_v3(eul1); + negate_v3(eul2); + } } /* Construct 4x4 matrix from Euler angles (in radians). */ void eulO_to_mat4(float mat[4][4], const float e[3], const short order) { - float unit_mat[3][3]; + float unit_mat[3][3]; - /* for now, we'll just do this the slow way (i.e. copying matrices) */ - eulO_to_mat3(unit_mat, e, order); - copy_m4_m3(mat, unit_mat); + /* for now, we'll just do this the slow way (i.e. copying matrices) */ + eulO_to_mat3(unit_mat, e, order); + copy_m4_m3(mat, unit_mat); } /* Convert 3x3 matrix to Euler angles (in radians). */ void mat3_normalized_to_eulO(float eul[3], const short order, const float m[3][3]) { - float eul1[3], eul2[3]; - float d1, d2; + float eul1[3], eul2[3]; + float d1, d2; - mat3_normalized_to_eulo2(m, eul1, eul2, order); + mat3_normalized_to_eulo2(m, eul1, eul2, order); - d1 = fabsf(eul1[0]) + fabsf(eul1[1]) + fabsf(eul1[2]); - d2 = fabsf(eul2[0]) + fabsf(eul2[1]) + fabsf(eul2[2]); + d1 = fabsf(eul1[0]) + fabsf(eul1[1]) + fabsf(eul1[2]); + d2 = fabsf(eul2[0]) + fabsf(eul2[1]) + fabsf(eul2[2]); - /* return best, which is just the one with lowest values it in */ - if (d1 > d2) { - copy_v3_v3(eul, eul2); - } - else { - copy_v3_v3(eul, eul1); - } + /* return best, which is just the one with lowest values it in */ + if (d1 > d2) { + copy_v3_v3(eul, eul2); + } + else { + copy_v3_v3(eul, eul1); + } } void mat3_to_eulO(float eul[3], const short order, const float m[3][3]) { - float unit_mat[3][3]; - normalize_m3_m3(unit_mat, m); - mat3_normalized_to_eulO(eul, order, unit_mat); + float unit_mat[3][3]; + normalize_m3_m3(unit_mat, m); + mat3_normalized_to_eulO(eul, order, unit_mat); } /* Convert 4x4 matrix to Euler angles (in radians). */ void mat4_normalized_to_eulO(float eul[3], const short order, const float m[4][4]) { - float mat3[3][3]; + float mat3[3][3]; - /* for now, we'll just do this the slow way (i.e. copying matrices) */ - copy_m3_m4(mat3, m); - mat3_normalized_to_eulO(eul, order, mat3); + /* for now, we'll just do this the slow way (i.e. copying matrices) */ + copy_m3_m4(mat3, m); + mat3_normalized_to_eulO(eul, order, mat3); } void mat4_to_eulO(float eul[3], const short order, const float m[4][4]) { - float mat3[3][3]; - copy_m3_m4(mat3, m); - normalize_m3(mat3); - mat3_normalized_to_eulO(eul, order, mat3); + float mat3[3][3]; + copy_m3_m4(mat3, m); + normalize_m3(mat3); + mat3_normalized_to_eulO(eul, order, mat3); } - /* uses 2 methods to retrieve eulers, and picks the closest */ -void mat3_normalized_to_compatible_eulO( - float eul[3], const float oldrot[3], const short order, const float mat[3][3]) +void mat3_normalized_to_compatible_eulO(float eul[3], + const float oldrot[3], + const short order, + const float mat[3][3]) { - float eul1[3], eul2[3]; - float d1, d2; + float eul1[3], eul2[3]; + float d1, d2; - mat3_normalized_to_eulo2(mat, eul1, eul2, order); + mat3_normalized_to_eulo2(mat, eul1, eul2, order); - compatible_eul(eul1, oldrot); - compatible_eul(eul2, oldrot); + compatible_eul(eul1, oldrot); + compatible_eul(eul2, oldrot); - d1 = fabsf(eul1[0] - oldrot[0]) + fabsf(eul1[1] - oldrot[1]) + fabsf(eul1[2] - oldrot[2]); - d2 = fabsf(eul2[0] - oldrot[0]) + fabsf(eul2[1] - oldrot[1]) + fabsf(eul2[2] - oldrot[2]); + d1 = fabsf(eul1[0] - oldrot[0]) + fabsf(eul1[1] - oldrot[1]) + fabsf(eul1[2] - oldrot[2]); + d2 = fabsf(eul2[0] - oldrot[0]) + fabsf(eul2[1] - oldrot[1]) + fabsf(eul2[2] - oldrot[2]); - /* return best, which is just the one with lowest difference */ - if (d1 > d2) { - copy_v3_v3(eul, eul2); - } - else { - copy_v3_v3(eul, eul1); - } + /* return best, which is just the one with lowest difference */ + if (d1 > d2) { + copy_v3_v3(eul, eul2); + } + else { + copy_v3_v3(eul, eul1); + } } -void mat3_to_compatible_eulO( - float eul[3], const float oldrot[3], const short order, const float mat[3][3]) +void mat3_to_compatible_eulO(float eul[3], + const float oldrot[3], + const short order, + const float mat[3][3]) { - float unit_mat[3][3]; + float unit_mat[3][3]; - normalize_m3_m3(unit_mat, mat); - mat3_normalized_to_compatible_eulO(eul, oldrot, order, unit_mat); + normalize_m3_m3(unit_mat, mat); + mat3_normalized_to_compatible_eulO(eul, oldrot, order, unit_mat); } -void mat4_normalized_to_compatible_eulO( - float eul[3], const float oldrot[3], const short order, const float m[4][4]) +void mat4_normalized_to_compatible_eulO(float eul[3], + const float oldrot[3], + const short order, + const float m[4][4]) { - float mat3[3][3]; + float mat3[3][3]; - /* for now, we'll just do this the slow way (i.e. copying matrices) */ - copy_m3_m4(mat3, m); - mat3_normalized_to_compatible_eulO(eul, oldrot, order, mat3); + /* for now, we'll just do this the slow way (i.e. copying matrices) */ + copy_m3_m4(mat3, m); + mat3_normalized_to_compatible_eulO(eul, oldrot, order, mat3); } -void mat4_to_compatible_eulO( - float eul[3], const float oldrot[3], const short order, const float m[4][4]) +void mat4_to_compatible_eulO(float eul[3], + const float oldrot[3], + const short order, + const float m[4][4]) { - float mat3[3][3]; + float mat3[3][3]; - /* for now, we'll just do this the slow way (i.e. copying matrices) */ - copy_m3_m4(mat3, m); - normalize_m3(mat3); - mat3_normalized_to_compatible_eulO(eul, oldrot, order, mat3); + /* for now, we'll just do this the slow way (i.e. copying matrices) */ + copy_m3_m4(mat3, m); + normalize_m3(mat3); + mat3_normalized_to_compatible_eulO(eul, oldrot, order, mat3); } -void quat_to_compatible_eulO( - float eul[3], const float oldrot[3], const short order, const float quat[4]) +void quat_to_compatible_eulO(float eul[3], + const float oldrot[3], + const short order, + const float quat[4]) { - float unit_mat[3][3]; + float unit_mat[3][3]; - quat_to_mat3(unit_mat, quat); - mat3_normalized_to_compatible_eulO(eul, oldrot, order, unit_mat); + quat_to_mat3(unit_mat, quat); + mat3_normalized_to_compatible_eulO(eul, oldrot, order, unit_mat); } /* rotate the given euler by the given angle on the specified axis */ @@ -1771,52 +1811,51 @@ void quat_to_compatible_eulO( void rotate_eulO(float beul[3], const short order, char axis, float ang) { - float eul[3], mat1[3][3], mat2[3][3], totmat[3][3]; + float eul[3], mat1[3][3], mat2[3][3], totmat[3][3]; - assert(axis >= 'X' && axis <= 'Z'); + assert(axis >= 'X' && axis <= 'Z'); - zero_v3(eul); + zero_v3(eul); - if (axis == 'X') { - eul[0] = ang; - } - else if (axis == 'Y') { - eul[1] = ang; - } - else { - eul[2] = ang; - } + if (axis == 'X') { + eul[0] = ang; + } + else if (axis == 'Y') { + eul[1] = ang; + } + else { + eul[2] = ang; + } - eulO_to_mat3(mat1, eul, order); - eulO_to_mat3(mat2, beul, order); + eulO_to_mat3(mat1, eul, order); + eulO_to_mat3(mat2, beul, order); - mul_m3_m3m3(totmat, mat2, mat1); + mul_m3_m3m3(totmat, mat2, mat1); - mat3_to_eulO(beul, order, totmat); + mat3_to_eulO(beul, order, totmat); } /* the matrix is written to as 3 axis vectors */ void eulO_to_gimbal_axis(float gmat[3][3], const float eul[3], const short order) { - const RotOrderInfo *R = get_rotation_order_info(order); - - float mat[3][3]; - float teul[3]; + const RotOrderInfo *R = get_rotation_order_info(order); - /* first axis is local */ - eulO_to_mat3(mat, eul, order); - copy_v3_v3(gmat[R->axis[0]], mat[R->axis[0]]); + float mat[3][3]; + float teul[3]; - /* second axis is local minus first rotation */ - copy_v3_v3(teul, eul); - teul[R->axis[0]] = 0; - eulO_to_mat3(mat, teul, order); - copy_v3_v3(gmat[R->axis[1]], mat[R->axis[1]]); + /* first axis is local */ + eulO_to_mat3(mat, eul, order); + copy_v3_v3(gmat[R->axis[0]], mat[R->axis[0]]); + /* second axis is local minus first rotation */ + copy_v3_v3(teul, eul); + teul[R->axis[0]] = 0; + eulO_to_mat3(mat, teul, order); + copy_v3_v3(gmat[R->axis[1]], mat[R->axis[1]]); - /* Last axis is global */ - zero_v3(gmat[R->axis[2]]); - gmat[R->axis[2]][R->axis[2]] = 1; + /* Last axis is global */ + zero_v3(gmat[R->axis[2]]); + gmat[R->axis[2]][R->axis[2]] = 1; } /******************************* Dual Quaternions ****************************/ @@ -1852,431 +1891,409 @@ void eulO_to_gimbal_axis(float gmat[3][3], const float eul[3], const short order void mat4_to_dquat(DualQuat *dq, const float basemat[4][4], const float mat[4][4]) { - float *t, *q, dscale[3], scale[3], basequat[4], mat3[3][3]; - float baseRS[4][4], baseinv[4][4], baseR[4][4], baseRinv[4][4]; - float R[4][4], S[4][4]; + float *t, *q, dscale[3], scale[3], basequat[4], mat3[3][3]; + float baseRS[4][4], baseinv[4][4], baseR[4][4], baseRinv[4][4]; + float R[4][4], S[4][4]; - /* split scaling and rotation, there is probably a faster way to do - * this, it's done like this now to correctly get negative scaling */ - mul_m4_m4m4(baseRS, mat, basemat); - mat4_to_size(scale, baseRS); + /* split scaling and rotation, there is probably a faster way to do + * this, it's done like this now to correctly get negative scaling */ + mul_m4_m4m4(baseRS, mat, basemat); + mat4_to_size(scale, baseRS); - dscale[0] = scale[0] - 1.0f; - dscale[1] = scale[1] - 1.0f; - dscale[2] = scale[2] - 1.0f; + dscale[0] = scale[0] - 1.0f; + dscale[1] = scale[1] - 1.0f; + dscale[2] = scale[2] - 1.0f; - copy_m3_m4(mat3, mat); + copy_m3_m4(mat3, mat); - if (!is_orthonormal_m3(mat3) || (determinant_m4(mat) < 0.0f) || len_squared_v3(dscale) > SQUARE(1e-4f)) { - /* extract R and S */ - float tmp[4][4]; + if (!is_orthonormal_m3(mat3) || (determinant_m4(mat) < 0.0f) || + len_squared_v3(dscale) > SQUARE(1e-4f)) { + /* extract R and S */ + float tmp[4][4]; - /* extra orthogonalize, to avoid flipping with stretched bones */ - copy_m4_m4(tmp, baseRS); - orthogonalize_m4(tmp, 1); - mat4_to_quat(basequat, tmp); + /* extra orthogonalize, to avoid flipping with stretched bones */ + copy_m4_m4(tmp, baseRS); + orthogonalize_m4(tmp, 1); + mat4_to_quat(basequat, tmp); - quat_to_mat4(baseR, basequat); - copy_v3_v3(baseR[3], baseRS[3]); + quat_to_mat4(baseR, basequat); + copy_v3_v3(baseR[3], baseRS[3]); - invert_m4_m4(baseinv, basemat); - mul_m4_m4m4(R, baseR, baseinv); + invert_m4_m4(baseinv, basemat); + mul_m4_m4m4(R, baseR, baseinv); - invert_m4_m4(baseRinv, baseR); - mul_m4_m4m4(S, baseRinv, baseRS); + invert_m4_m4(baseRinv, baseR); + mul_m4_m4m4(S, baseRinv, baseRS); - /* set scaling part */ - mul_m4_series(dq->scale, basemat, S, baseinv); - dq->scale_weight = 1.0f; - } - else { - /* matrix does not contain scaling */ - copy_m4_m4(R, mat); - dq->scale_weight = 0.0f; - } + /* set scaling part */ + mul_m4_series(dq->scale, basemat, S, baseinv); + dq->scale_weight = 1.0f; + } + else { + /* matrix does not contain scaling */ + copy_m4_m4(R, mat); + dq->scale_weight = 0.0f; + } - /* non-dual part */ - mat4_to_quat(dq->quat, R); + /* non-dual part */ + mat4_to_quat(dq->quat, R); - /* dual part */ - t = R[3]; - q = dq->quat; - dq->trans[0] = -0.5f * ( t[0] * q[1] + t[1] * q[2] + t[2] * q[3]); - dq->trans[1] = 0.5f * ( t[0] * q[0] + t[1] * q[3] - t[2] * q[2]); - dq->trans[2] = 0.5f * (-t[0] * q[3] + t[1] * q[0] + t[2] * q[1]); - dq->trans[3] = 0.5f * ( t[0] * q[2] - t[1] * q[1] + t[2] * q[0]); + /* dual part */ + t = R[3]; + q = dq->quat; + dq->trans[0] = -0.5f * (t[0] * q[1] + t[1] * q[2] + t[2] * q[3]); + dq->trans[1] = 0.5f * (t[0] * q[0] + t[1] * q[3] - t[2] * q[2]); + dq->trans[2] = 0.5f * (-t[0] * q[3] + t[1] * q[0] + t[2] * q[1]); + dq->trans[3] = 0.5f * (t[0] * q[2] - t[1] * q[1] + t[2] * q[0]); } void dquat_to_mat4(float mat[4][4], const DualQuat *dq) { - float len, q0[4]; - const float *t; + float len, q0[4]; + const float *t; - /* regular quaternion */ - copy_qt_qt(q0, dq->quat); + /* regular quaternion */ + copy_qt_qt(q0, dq->quat); - /* normalize */ - len = sqrtf(dot_qtqt(q0, q0)); - if (len != 0.0f) { - len = 1.0f / len; - } - mul_qt_fl(q0, len); + /* normalize */ + len = sqrtf(dot_qtqt(q0, q0)); + if (len != 0.0f) { + len = 1.0f / len; + } + mul_qt_fl(q0, len); - /* rotation */ - quat_to_mat4(mat, q0); + /* rotation */ + quat_to_mat4(mat, q0); - /* translation */ - t = dq->trans; - mat[3][0] = 2.0f * (-t[0] * q0[1] + t[1] * q0[0] - t[2] * q0[3] + t[3] * q0[2]) * len; - mat[3][1] = 2.0f * (-t[0] * q0[2] + t[1] * q0[3] + t[2] * q0[0] - t[3] * q0[1]) * len; - mat[3][2] = 2.0f * (-t[0] * q0[3] - t[1] * q0[2] + t[2] * q0[1] + t[3] * q0[0]) * len; + /* translation */ + t = dq->trans; + mat[3][0] = 2.0f * (-t[0] * q0[1] + t[1] * q0[0] - t[2] * q0[3] + t[3] * q0[2]) * len; + mat[3][1] = 2.0f * (-t[0] * q0[2] + t[1] * q0[3] + t[2] * q0[0] - t[3] * q0[1]) * len; + mat[3][2] = 2.0f * (-t[0] * q0[3] - t[1] * q0[2] + t[2] * q0[1] + t[3] * q0[0]) * len; - /* scaling */ - if (dq->scale_weight) { - mul_m4_m4m4(mat, mat, dq->scale); - } + /* scaling */ + if (dq->scale_weight) { + mul_m4_m4m4(mat, mat, dq->scale); + } } void add_weighted_dq_dq(DualQuat *dqsum, const DualQuat *dq, float weight) { - bool flipped = false; + bool flipped = false; - /* make sure we interpolate quats in the right direction */ - if (dot_qtqt(dq->quat, dqsum->quat) < 0) { - flipped = true; - weight = -weight; - } + /* make sure we interpolate quats in the right direction */ + if (dot_qtqt(dq->quat, dqsum->quat) < 0) { + flipped = true; + weight = -weight; + } - /* interpolate rotation and translation */ - dqsum->quat[0] += weight * dq->quat[0]; - dqsum->quat[1] += weight * dq->quat[1]; - dqsum->quat[2] += weight * dq->quat[2]; - dqsum->quat[3] += weight * dq->quat[3]; + /* interpolate rotation and translation */ + dqsum->quat[0] += weight * dq->quat[0]; + dqsum->quat[1] += weight * dq->quat[1]; + dqsum->quat[2] += weight * dq->quat[2]; + dqsum->quat[3] += weight * dq->quat[3]; - dqsum->trans[0] += weight * dq->trans[0]; - dqsum->trans[1] += weight * dq->trans[1]; - dqsum->trans[2] += weight * dq->trans[2]; - dqsum->trans[3] += weight * dq->trans[3]; + dqsum->trans[0] += weight * dq->trans[0]; + dqsum->trans[1] += weight * dq->trans[1]; + dqsum->trans[2] += weight * dq->trans[2]; + dqsum->trans[3] += weight * dq->trans[3]; - /* Interpolate scale - but only if there is scale present. If any dual - * quaternions without scale are added, they will be compensated for in - * normalize_dq. */ - if (dq->scale_weight) { - float wmat[4][4]; + /* Interpolate scale - but only if there is scale present. If any dual + * quaternions without scale are added, they will be compensated for in + * normalize_dq. */ + if (dq->scale_weight) { + float wmat[4][4]; - if (flipped) { - /* we don't want negative weights for scaling */ - weight = -weight; - } + if (flipped) { + /* we don't want negative weights for scaling */ + weight = -weight; + } - copy_m4_m4(wmat, (float(*)[4])dq->scale); - mul_m4_fl(wmat, weight); - add_m4_m4m4(dqsum->scale, dqsum->scale, wmat); - dqsum->scale_weight += weight; - } + copy_m4_m4(wmat, (float(*)[4])dq->scale); + mul_m4_fl(wmat, weight); + add_m4_m4m4(dqsum->scale, dqsum->scale, wmat); + dqsum->scale_weight += weight; + } } void normalize_dq(DualQuat *dq, float totweight) { - const float scale = 1.0f / totweight; + const float scale = 1.0f / totweight; - mul_qt_fl(dq->quat, scale); - mul_qt_fl(dq->trans, scale); + mul_qt_fl(dq->quat, scale); + mul_qt_fl(dq->trans, scale); - /* Handle scale if needed. */ - if (dq->scale_weight) { - /* Compensate for any dual quaternions added without scale. This is an - * optimization so that we can skip the scale part when not needed. */ - float addweight = totweight - dq->scale_weight; + /* Handle scale if needed. */ + if (dq->scale_weight) { + /* Compensate for any dual quaternions added without scale. This is an + * optimization so that we can skip the scale part when not needed. */ + float addweight = totweight - dq->scale_weight; - if (addweight) { - dq->scale[0][0] += addweight; - dq->scale[1][1] += addweight; - dq->scale[2][2] += addweight; - dq->scale[3][3] += addweight; - } + if (addweight) { + dq->scale[0][0] += addweight; + dq->scale[1][1] += addweight; + dq->scale[2][2] += addweight; + dq->scale[3][3] += addweight; + } - mul_m4_fl(dq->scale, scale); - dq->scale_weight = 1.0f; - } + mul_m4_fl(dq->scale, scale); + dq->scale_weight = 1.0f; + } } void mul_v3m3_dq(float co[3], float mat[3][3], DualQuat *dq) { - float M[3][3], t[3], scalemat[3][3], len2; - float w = dq->quat[0], x = dq->quat[1], y = dq->quat[2], z = dq->quat[3]; - float t0 = dq->trans[0], t1 = dq->trans[1], t2 = dq->trans[2], t3 = dq->trans[3]; - - /* rotation matrix */ - M[0][0] = w * w + x * x - y * y - z * z; - M[1][0] = 2 * (x * y - w * z); - M[2][0] = 2 * (x * z + w * y); - - M[0][1] = 2 * (x * y + w * z); - M[1][1] = w * w + y * y - x * x - z * z; - M[2][1] = 2 * (y * z - w * x); - - M[0][2] = 2 * (x * z - w * y); - M[1][2] = 2 * (y * z + w * x); - M[2][2] = w * w + z * z - x * x - y * y; - - len2 = dot_qtqt(dq->quat, dq->quat); - if (len2 > 0.0f) { - len2 = 1.0f / len2; - } - - /* translation */ - t[0] = 2 * (-t0 * x + w * t1 - t2 * z + y * t3); - t[1] = 2 * (-t0 * y + t1 * z - x * t3 + w * t2); - t[2] = 2 * (-t0 * z + x * t2 + w * t3 - t1 * y); - - /* apply scaling */ - if (dq->scale_weight) { - mul_m4_v3(dq->scale, co); - } - - /* apply rotation and translation */ - mul_m3_v3(M, co); - co[0] = (co[0] + t[0]) * len2; - co[1] = (co[1] + t[1]) * len2; - co[2] = (co[2] + t[2]) * len2; - - /* compute crazyspace correction mat */ - if (mat) { - if (dq->scale_weight) { - copy_m3_m4(scalemat, dq->scale); - mul_m3_m3m3(mat, M, scalemat); - } - else { - copy_m3_m3(mat, M); - } - mul_m3_fl(mat, len2); - } + float M[3][3], t[3], scalemat[3][3], len2; + float w = dq->quat[0], x = dq->quat[1], y = dq->quat[2], z = dq->quat[3]; + float t0 = dq->trans[0], t1 = dq->trans[1], t2 = dq->trans[2], t3 = dq->trans[3]; + + /* rotation matrix */ + M[0][0] = w * w + x * x - y * y - z * z; + M[1][0] = 2 * (x * y - w * z); + M[2][0] = 2 * (x * z + w * y); + + M[0][1] = 2 * (x * y + w * z); + M[1][1] = w * w + y * y - x * x - z * z; + M[2][1] = 2 * (y * z - w * x); + + M[0][2] = 2 * (x * z - w * y); + M[1][2] = 2 * (y * z + w * x); + M[2][2] = w * w + z * z - x * x - y * y; + + len2 = dot_qtqt(dq->quat, dq->quat); + if (len2 > 0.0f) { + len2 = 1.0f / len2; + } + + /* translation */ + t[0] = 2 * (-t0 * x + w * t1 - t2 * z + y * t3); + t[1] = 2 * (-t0 * y + t1 * z - x * t3 + w * t2); + t[2] = 2 * (-t0 * z + x * t2 + w * t3 - t1 * y); + + /* apply scaling */ + if (dq->scale_weight) { + mul_m4_v3(dq->scale, co); + } + + /* apply rotation and translation */ + mul_m3_v3(M, co); + co[0] = (co[0] + t[0]) * len2; + co[1] = (co[1] + t[1]) * len2; + co[2] = (co[2] + t[2]) * len2; + + /* compute crazyspace correction mat */ + if (mat) { + if (dq->scale_weight) { + copy_m3_m4(scalemat, dq->scale); + mul_m3_m3m3(mat, M, scalemat); + } + else { + copy_m3_m3(mat, M); + } + mul_m3_fl(mat, len2); + } } void copy_dq_dq(DualQuat *dq1, const DualQuat *dq2) { - memcpy(dq1, dq2, sizeof(DualQuat)); + memcpy(dq1, dq2, sizeof(DualQuat)); } /* axis matches eTrackToAxis_Modes */ void quat_apply_track(float quat[4], short axis, short upflag) { - /* rotations are hard coded to match vec_to_quat */ - const float sqrt_1_2 = (float)M_SQRT1_2; - const float quat_track[][4] = { - /* pos-y90 */ - {sqrt_1_2, 0.0, -sqrt_1_2, 0.0}, - /* Quaternion((1,0,0), radians(90)) * Quaternion((0,1,0), radians(90)) */ - {0.5, 0.5, 0.5, 0.5}, - /* pos-z90 */ - {sqrt_1_2, 0.0, 0.0, sqrt_1_2}, - /* neg-y90 */ - {sqrt_1_2, 0.0, sqrt_1_2, 0.0}, - /* Quaternion((1,0,0), radians(-90)) * Quaternion((0,1,0), radians(-90)) */ - {0.5, -0.5, -0.5, 0.5}, - /* no rotation */ - {0.0, sqrt_1_2, sqrt_1_2, 0.0}, - }; - - assert(axis >= 0 && axis <= 5); - assert(upflag >= 0 && upflag <= 2); - - mul_qt_qtqt(quat, quat, quat_track[axis]); - - if (axis > 2) { - axis = (short)(axis - 3); - } - - /* there are 2 possible up-axis for each axis used, the 'quat_track' applies so the first - * up axis is used X->Y, Y->X, Z->X, if this first up axis isn't used then rotate 90d - * the strange bit shift below just find the low axis {X:Y, Y:X, Z:X} */ - if (upflag != (2 - axis) >> 1) { - float q[4] = {sqrt_1_2, 0.0, 0.0, 0.0}; /* assign 90d rotation axis */ - q[axis + 1] = ((axis == 1)) ? sqrt_1_2 : -sqrt_1_2; /* flip non Y axis */ - mul_qt_qtqt(quat, quat, q); - } + /* rotations are hard coded to match vec_to_quat */ + const float sqrt_1_2 = (float)M_SQRT1_2; + const float quat_track[][4] = { + /* pos-y90 */ + {sqrt_1_2, 0.0, -sqrt_1_2, 0.0}, + /* Quaternion((1,0,0), radians(90)) * Quaternion((0,1,0), radians(90)) */ + {0.5, 0.5, 0.5, 0.5}, + /* pos-z90 */ + {sqrt_1_2, 0.0, 0.0, sqrt_1_2}, + /* neg-y90 */ + {sqrt_1_2, 0.0, sqrt_1_2, 0.0}, + /* Quaternion((1,0,0), radians(-90)) * Quaternion((0,1,0), radians(-90)) */ + {0.5, -0.5, -0.5, 0.5}, + /* no rotation */ + {0.0, sqrt_1_2, sqrt_1_2, 0.0}, + }; + + assert(axis >= 0 && axis <= 5); + assert(upflag >= 0 && upflag <= 2); + + mul_qt_qtqt(quat, quat, quat_track[axis]); + + if (axis > 2) { + axis = (short)(axis - 3); + } + + /* there are 2 possible up-axis for each axis used, the 'quat_track' applies so the first + * up axis is used X->Y, Y->X, Z->X, if this first up axis isn't used then rotate 90d + * the strange bit shift below just find the low axis {X:Y, Y:X, Z:X} */ + if (upflag != (2 - axis) >> 1) { + float q[4] = {sqrt_1_2, 0.0, 0.0, 0.0}; /* assign 90d rotation axis */ + q[axis + 1] = ((axis == 1)) ? sqrt_1_2 : -sqrt_1_2; /* flip non Y axis */ + mul_qt_qtqt(quat, quat, q); + } } void vec_apply_track(float vec[3], short axis) { - float tvec[3]; - - assert(axis >= 0 && axis <= 5); - - copy_v3_v3(tvec, vec); - - switch (axis) { - case 0: /* pos-x */ - /* vec[0] = 0.0; */ - vec[1] = tvec[2]; - vec[2] = -tvec[1]; - break; - case 1: /* pos-y */ - /* vec[0] = tvec[0]; */ - /* vec[1] = 0.0; */ - /* vec[2] = tvec[2]; */ - break; - case 2: /* pos-z */ - /* vec[0] = tvec[0]; */ - /* vec[1] = tvec[1]; */ - /* vec[2] = 0.0; */ - break; - case 3: /* neg-x */ - /* vec[0] = 0.0; */ - vec[1] = tvec[2]; - vec[2] = -tvec[1]; - break; - case 4: /* neg-y */ - vec[0] = -tvec[2]; - /* vec[1] = 0.0; */ - vec[2] = tvec[0]; - break; - case 5: /* neg-z */ - vec[0] = -tvec[0]; - vec[1] = -tvec[1]; - /* vec[2] = 0.0; */ - break; - } + float tvec[3]; + + assert(axis >= 0 && axis <= 5); + + copy_v3_v3(tvec, vec); + + switch (axis) { + case 0: /* pos-x */ + /* vec[0] = 0.0; */ + vec[1] = tvec[2]; + vec[2] = -tvec[1]; + break; + case 1: /* pos-y */ + /* vec[0] = tvec[0]; */ + /* vec[1] = 0.0; */ + /* vec[2] = tvec[2]; */ + break; + case 2: /* pos-z */ + /* vec[0] = tvec[0]; */ + /* vec[1] = tvec[1]; */ + /* vec[2] = 0.0; */ + break; + case 3: /* neg-x */ + /* vec[0] = 0.0; */ + vec[1] = tvec[2]; + vec[2] = -tvec[1]; + break; + case 4: /* neg-y */ + vec[0] = -tvec[2]; + /* vec[1] = 0.0; */ + vec[2] = tvec[0]; + break; + case 5: /* neg-z */ + vec[0] = -tvec[0]; + vec[1] = -tvec[1]; + /* vec[2] = 0.0; */ + break; + } } /* lens/angle conversion (radians) */ float focallength_to_fov(float focal_length, float sensor) { - return 2.0f * atanf((sensor / 2.0f) / focal_length); + return 2.0f * atanf((sensor / 2.0f) / focal_length); } float fov_to_focallength(float hfov, float sensor) { - return (sensor / 2.0f) / tanf(hfov * 0.5f); + return (sensor / 2.0f) / tanf(hfov * 0.5f); } /* 'mod_inline(-3, 4)= 1', 'fmod(-3, 4)= -3' */ static float mod_inline(float a, float b) { - return a - (b * floorf(a / b)); + return a - (b * floorf(a / b)); } float angle_wrap_rad(float angle) { - return mod_inline(angle + (float)M_PI, (float)M_PI * 2.0f) - (float)M_PI; + return mod_inline(angle + (float)M_PI, (float)M_PI * 2.0f) - (float)M_PI; } float angle_wrap_deg(float angle) { - return mod_inline(angle + 180.0f, 360.0f) - 180.0f; + return mod_inline(angle + 180.0f, 360.0f) - 180.0f; } /* returns an angle compatible with angle_compat */ float angle_compat_rad(float angle, float angle_compat) { - return angle_compat + angle_wrap_rad(angle - angle_compat); + return angle_compat + angle_wrap_rad(angle - angle_compat); } /* axis conversion */ static float _axis_convert_matrix[23][3][3] = { - {{-1.0, 0.0, 0.0}, {0.0, -1.0, 0.0}, {0.0, 0.0, 1.0}}, - {{-1.0, 0.0, 0.0}, {0.0, 0.0, -1.0}, {0.0, -1.0, 0.0}}, - {{-1.0, 0.0, 0.0}, {0.0, 0.0, 1.0}, {0.0, 1.0, 0.0}}, - {{-1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, -1.0}}, - {{0.0, -1.0, 0.0}, {-1.0, 0.0, 0.0}, {0.0, 0.0, -1.0}}, - {{0.0, 0.0, 1.0}, {-1.0, 0.0, 0.0}, {0.0, -1.0, 0.0}}, - {{0.0, 0.0, -1.0}, {-1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}}, - {{0.0, 1.0, 0.0}, {-1.0, 0.0, 0.0}, {0.0, 0.0, 1.0}}, - {{0.0, -1.0, 0.0}, {0.0, 0.0, 1.0}, {-1.0, 0.0, 0.0}}, - {{0.0, 0.0, -1.0}, {0.0, -1.0, 0.0}, {-1.0, 0.0, 0.0}}, - {{0.0, 0.0, 1.0}, {0.0, 1.0, 0.0}, {-1.0, 0.0, 0.0}}, - {{0.0, 1.0, 0.0}, {0.0, 0.0, -1.0}, {-1.0, 0.0, 0.0}}, - {{0.0, -1.0, 0.0}, {0.0, 0.0, -1.0}, {1.0, 0.0, 0.0}}, - {{0.0, 0.0, 1.0}, {0.0, -1.0, 0.0}, {1.0, 0.0, 0.0}}, - {{0.0, 0.0, -1.0}, {0.0, 1.0, 0.0}, {1.0, 0.0, 0.0}}, - {{0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}, {1.0, 0.0, 0.0}}, - {{0.0, -1.0, 0.0}, {1.0, 0.0, 0.0}, {0.0, 0.0, 1.0}}, - {{0.0, 0.0, -1.0}, {1.0, 0.0, 0.0}, {0.0, -1.0, 0.0}}, - {{0.0, 0.0, 1.0}, {1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}}, - {{0.0, 1.0, 0.0}, {1.0, 0.0, 0.0}, {0.0, 0.0, -1.0}}, - {{1.0, 0.0, 0.0}, {0.0, -1.0, 0.0}, {0.0, 0.0, -1.0}}, - {{1.0, 0.0, 0.0}, {0.0, 0.0, 1.0}, {0.0, -1.0, 0.0}}, - {{1.0, 0.0, 0.0}, {0.0, 0.0, -1.0}, {0.0, 1.0, 0.0}}, + {{-1.0, 0.0, 0.0}, {0.0, -1.0, 0.0}, {0.0, 0.0, 1.0}}, + {{-1.0, 0.0, 0.0}, {0.0, 0.0, -1.0}, {0.0, -1.0, 0.0}}, + {{-1.0, 0.0, 0.0}, {0.0, 0.0, 1.0}, {0.0, 1.0, 0.0}}, + {{-1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, -1.0}}, + {{0.0, -1.0, 0.0}, {-1.0, 0.0, 0.0}, {0.0, 0.0, -1.0}}, + {{0.0, 0.0, 1.0}, {-1.0, 0.0, 0.0}, {0.0, -1.0, 0.0}}, + {{0.0, 0.0, -1.0}, {-1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}}, + {{0.0, 1.0, 0.0}, {-1.0, 0.0, 0.0}, {0.0, 0.0, 1.0}}, + {{0.0, -1.0, 0.0}, {0.0, 0.0, 1.0}, {-1.0, 0.0, 0.0}}, + {{0.0, 0.0, -1.0}, {0.0, -1.0, 0.0}, {-1.0, 0.0, 0.0}}, + {{0.0, 0.0, 1.0}, {0.0, 1.0, 0.0}, {-1.0, 0.0, 0.0}}, + {{0.0, 1.0, 0.0}, {0.0, 0.0, -1.0}, {-1.0, 0.0, 0.0}}, + {{0.0, -1.0, 0.0}, {0.0, 0.0, -1.0}, {1.0, 0.0, 0.0}}, + {{0.0, 0.0, 1.0}, {0.0, -1.0, 0.0}, {1.0, 0.0, 0.0}}, + {{0.0, 0.0, -1.0}, {0.0, 1.0, 0.0}, {1.0, 0.0, 0.0}}, + {{0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}, {1.0, 0.0, 0.0}}, + {{0.0, -1.0, 0.0}, {1.0, 0.0, 0.0}, {0.0, 0.0, 1.0}}, + {{0.0, 0.0, -1.0}, {1.0, 0.0, 0.0}, {0.0, -1.0, 0.0}}, + {{0.0, 0.0, 1.0}, {1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}}, + {{0.0, 1.0, 0.0}, {1.0, 0.0, 0.0}, {0.0, 0.0, -1.0}}, + {{1.0, 0.0, 0.0}, {0.0, -1.0, 0.0}, {0.0, 0.0, -1.0}}, + {{1.0, 0.0, 0.0}, {0.0, 0.0, 1.0}, {0.0, -1.0, 0.0}}, + {{1.0, 0.0, 0.0}, {0.0, 0.0, -1.0}, {0.0, 1.0, 0.0}}, }; static int _axis_convert_lut[23][24] = { - {0x8C8, 0x4D0, 0x2E0, 0xAE8, 0x701, 0x511, 0x119, 0xB29, 0x682, 0x88A, - 0x09A, 0x2A2, 0x80B, 0x413, 0x223, 0xA2B, 0x644, 0x454, 0x05C, 0xA6C, - 0x745, 0x94D, 0x15D, 0x365}, - {0xAC8, 0x8D0, 0x4E0, 0x2E8, 0x741, 0x951, 0x159, 0x369, 0x702, 0xB0A, - 0x11A, 0x522, 0xA0B, 0x813, 0x423, 0x22B, 0x684, 0x894, 0x09C, 0x2AC, - 0x645, 0xA4D, 0x05D, 0x465}, - {0x4C8, 0x2D0, 0xAE0, 0x8E8, 0x681, 0x291, 0x099, 0x8A9, 0x642, 0x44A, - 0x05A, 0xA62, 0x40B, 0x213, 0xA23, 0x82B, 0x744, 0x354, 0x15C, 0x96C, - 0x705, 0x50D, 0x11D, 0xB25}, - {0x2C8, 0xAD0, 0x8E0, 0x4E8, 0x641, 0xA51, 0x059, 0x469, 0x742, 0x34A, - 0x15A, 0x962, 0x20B, 0xA13, 0x823, 0x42B, 0x704, 0xB14, 0x11C, 0x52C, - 0x685, 0x28D, 0x09D, 0x8A5}, - {0x708, 0xB10, 0x120, 0x528, 0x8C1, 0xAD1, 0x2D9, 0x4E9, 0x942, 0x74A, - 0x35A, 0x162, 0x64B, 0xA53, 0x063, 0x46B, 0x804, 0xA14, 0x21C, 0x42C, - 0x885, 0x68D, 0x29D, 0x0A5}, - {0xB08, 0x110, 0x520, 0x728, 0x941, 0x151, 0x359, 0x769, 0x802, 0xA0A, - 0x21A, 0x422, 0xA4B, 0x053, 0x463, 0x66B, 0x884, 0x094, 0x29C, 0x6AC, - 0x8C5, 0xACD, 0x2DD, 0x4E5}, - {0x508, 0x710, 0xB20, 0x128, 0x881, 0x691, 0x299, 0x0A9, 0x8C2, 0x4CA, - 0x2DA, 0xAE2, 0x44B, 0x653, 0xA63, 0x06B, 0x944, 0x754, 0x35C, 0x16C, - 0x805, 0x40D, 0x21D, 0xA25}, - {0x108, 0x510, 0x720, 0xB28, 0x801, 0x411, 0x219, 0xA29, 0x882, 0x08A, - 0x29A, 0x6A2, 0x04B, 0x453, 0x663, 0xA6B, 0x8C4, 0x4D4, 0x2DC, 0xAEC, - 0x945, 0x14D, 0x35D, 0x765}, - {0x748, 0x350, 0x160, 0x968, 0xAC1, 0x2D1, 0x4D9, 0x8E9, 0xA42, 0x64A, - 0x45A, 0x062, 0x68B, 0x293, 0x0A3, 0x8AB, 0xA04, 0x214, 0x41C, 0x82C, - 0xB05, 0x70D, 0x51D, 0x125}, - {0x948, 0x750, 0x360, 0x168, 0xB01, 0x711, 0x519, 0x129, 0xAC2, 0x8CA, - 0x4DA, 0x2E2, 0x88B, 0x693, 0x2A3, 0x0AB, 0xA44, 0x654, 0x45C, 0x06C, - 0xA05, 0x80D, 0x41D, 0x225}, - {0x348, 0x150, 0x960, 0x768, 0xA41, 0x051, 0x459, 0x669, 0xA02, 0x20A, - 0x41A, 0x822, 0x28B, 0x093, 0x8A3, 0x6AB, 0xB04, 0x114, 0x51C, 0x72C, - 0xAC5, 0x2CD, 0x4DD, 0x8E5}, - {0x148, 0x950, 0x760, 0x368, 0xA01, 0x811, 0x419, 0x229, 0xB02, 0x10A, - 0x51A, 0x722, 0x08B, 0x893, 0x6A3, 0x2AB, 0xAC4, 0x8D4, 0x4DC, 0x2EC, - 0xA45, 0x04D, 0x45D, 0x665}, - {0x688, 0x890, 0x0A0, 0x2A8, 0x4C1, 0x8D1, 0xAD9, 0x2E9, 0x502, 0x70A, - 0xB1A, 0x122, 0x74B, 0x953, 0x163, 0x36B, 0x404, 0x814, 0xA1C, 0x22C, - 0x445, 0x64D, 0xA5D, 0x065}, - {0x888, 0x090, 0x2A0, 0x6A8, 0x501, 0x111, 0xB19, 0x729, 0x402, 0x80A, - 0xA1A, 0x222, 0x94B, 0x153, 0x363, 0x76B, 0x444, 0x054, 0xA5C, 0x66C, - 0x4C5, 0x8CD, 0xADD, 0x2E5}, - {0x288, 0x690, 0x8A0, 0x0A8, 0x441, 0x651, 0xA59, 0x069, 0x4C2, 0x2CA, - 0xADA, 0x8E2, 0x34B, 0x753, 0x963, 0x16B, 0x504, 0x714, 0xB1C, 0x12C, - 0x405, 0x20D, 0xA1D, 0x825}, - {0x088, 0x290, 0x6A0, 0x8A8, 0x401, 0x211, 0xA19, 0x829, 0x442, 0x04A, - 0xA5A, 0x662, 0x14B, 0x353, 0x763, 0x96B, 0x4C4, 0x2D4, 0xADC, 0x8EC, - 0x505, 0x10D, 0xB1D, 0x725}, - {0x648, 0x450, 0x060, 0xA68, 0x2C1, 0x4D1, 0x8D9, 0xAE9, 0x282, 0x68A, - 0x89A, 0x0A2, 0x70B, 0x513, 0x123, 0xB2B, 0x204, 0x414, 0x81C, 0xA2C, - 0x345, 0x74D, 0x95D, 0x165}, - {0xA48, 0x650, 0x460, 0x068, 0x341, 0x751, 0x959, 0x169, 0x2C2, 0xACA, - 0x8DA, 0x4E2, 0xB0B, 0x713, 0x523, 0x12B, 0x284, 0x694, 0x89C, 0x0AC, - 0x205, 0xA0D, 0x81D, 0x425}, - {0x448, 0x050, 0xA60, 0x668, 0x281, 0x091, 0x899, 0x6A9, 0x202, 0x40A, - 0x81A, 0xA22, 0x50B, 0x113, 0xB23, 0x72B, 0x344, 0x154, 0x95C, 0x76C, - 0x2C5, 0x4CD, 0x8DD, 0xAE5}, - {0x048, 0xA50, 0x660, 0x468, 0x201, 0xA11, 0x819, 0x429, 0x342, 0x14A, - 0x95A, 0x762, 0x10B, 0xB13, 0x723, 0x52B, 0x2C4, 0xAD4, 0x8DC, 0x4EC, - 0x285, 0x08D, 0x89D, 0x6A5}, - {0x808, 0xA10, 0x220, 0x428, 0x101, 0xB11, 0x719, 0x529, 0x142, 0x94A, - 0x75A, 0x362, 0x8CB, 0xAD3, 0x2E3, 0x4EB, 0x044, 0xA54, 0x65C, 0x46C, - 0x085, 0x88D, 0x69D, 0x2A5}, - {0xA08, 0x210, 0x420, 0x828, 0x141, 0x351, 0x759, 0x969, 0x042, 0xA4A, - 0x65A, 0x462, 0xACB, 0x2D3, 0x4E3, 0x8EB, 0x084, 0x294, 0x69C, 0x8AC, - 0x105, 0xB0D, 0x71D, 0x525}, - {0x408, 0x810, 0xA20, 0x228, 0x081, 0x891, 0x699, 0x2A9, 0x102, 0x50A, - 0x71A, 0xB22, 0x4CB, 0x8D3, 0xAE3, 0x2EB, 0x144, 0x954, 0x75C, 0x36C, - 0x045, 0x44D, 0x65D, 0xA65}, + {0x8C8, 0x4D0, 0x2E0, 0xAE8, 0x701, 0x511, 0x119, 0xB29, 0x682, 0x88A, 0x09A, 0x2A2, + 0x80B, 0x413, 0x223, 0xA2B, 0x644, 0x454, 0x05C, 0xA6C, 0x745, 0x94D, 0x15D, 0x365}, + {0xAC8, 0x8D0, 0x4E0, 0x2E8, 0x741, 0x951, 0x159, 0x369, 0x702, 0xB0A, 0x11A, 0x522, + 0xA0B, 0x813, 0x423, 0x22B, 0x684, 0x894, 0x09C, 0x2AC, 0x645, 0xA4D, 0x05D, 0x465}, + {0x4C8, 0x2D0, 0xAE0, 0x8E8, 0x681, 0x291, 0x099, 0x8A9, 0x642, 0x44A, 0x05A, 0xA62, + 0x40B, 0x213, 0xA23, 0x82B, 0x744, 0x354, 0x15C, 0x96C, 0x705, 0x50D, 0x11D, 0xB25}, + {0x2C8, 0xAD0, 0x8E0, 0x4E8, 0x641, 0xA51, 0x059, 0x469, 0x742, 0x34A, 0x15A, 0x962, + 0x20B, 0xA13, 0x823, 0x42B, 0x704, 0xB14, 0x11C, 0x52C, 0x685, 0x28D, 0x09D, 0x8A5}, + {0x708, 0xB10, 0x120, 0x528, 0x8C1, 0xAD1, 0x2D9, 0x4E9, 0x942, 0x74A, 0x35A, 0x162, + 0x64B, 0xA53, 0x063, 0x46B, 0x804, 0xA14, 0x21C, 0x42C, 0x885, 0x68D, 0x29D, 0x0A5}, + {0xB08, 0x110, 0x520, 0x728, 0x941, 0x151, 0x359, 0x769, 0x802, 0xA0A, 0x21A, 0x422, + 0xA4B, 0x053, 0x463, 0x66B, 0x884, 0x094, 0x29C, 0x6AC, 0x8C5, 0xACD, 0x2DD, 0x4E5}, + {0x508, 0x710, 0xB20, 0x128, 0x881, 0x691, 0x299, 0x0A9, 0x8C2, 0x4CA, 0x2DA, 0xAE2, + 0x44B, 0x653, 0xA63, 0x06B, 0x944, 0x754, 0x35C, 0x16C, 0x805, 0x40D, 0x21D, 0xA25}, + {0x108, 0x510, 0x720, 0xB28, 0x801, 0x411, 0x219, 0xA29, 0x882, 0x08A, 0x29A, 0x6A2, + 0x04B, 0x453, 0x663, 0xA6B, 0x8C4, 0x4D4, 0x2DC, 0xAEC, 0x945, 0x14D, 0x35D, 0x765}, + {0x748, 0x350, 0x160, 0x968, 0xAC1, 0x2D1, 0x4D9, 0x8E9, 0xA42, 0x64A, 0x45A, 0x062, + 0x68B, 0x293, 0x0A3, 0x8AB, 0xA04, 0x214, 0x41C, 0x82C, 0xB05, 0x70D, 0x51D, 0x125}, + {0x948, 0x750, 0x360, 0x168, 0xB01, 0x711, 0x519, 0x129, 0xAC2, 0x8CA, 0x4DA, 0x2E2, + 0x88B, 0x693, 0x2A3, 0x0AB, 0xA44, 0x654, 0x45C, 0x06C, 0xA05, 0x80D, 0x41D, 0x225}, + {0x348, 0x150, 0x960, 0x768, 0xA41, 0x051, 0x459, 0x669, 0xA02, 0x20A, 0x41A, 0x822, + 0x28B, 0x093, 0x8A3, 0x6AB, 0xB04, 0x114, 0x51C, 0x72C, 0xAC5, 0x2CD, 0x4DD, 0x8E5}, + {0x148, 0x950, 0x760, 0x368, 0xA01, 0x811, 0x419, 0x229, 0xB02, 0x10A, 0x51A, 0x722, + 0x08B, 0x893, 0x6A3, 0x2AB, 0xAC4, 0x8D4, 0x4DC, 0x2EC, 0xA45, 0x04D, 0x45D, 0x665}, + {0x688, 0x890, 0x0A0, 0x2A8, 0x4C1, 0x8D1, 0xAD9, 0x2E9, 0x502, 0x70A, 0xB1A, 0x122, + 0x74B, 0x953, 0x163, 0x36B, 0x404, 0x814, 0xA1C, 0x22C, 0x445, 0x64D, 0xA5D, 0x065}, + {0x888, 0x090, 0x2A0, 0x6A8, 0x501, 0x111, 0xB19, 0x729, 0x402, 0x80A, 0xA1A, 0x222, + 0x94B, 0x153, 0x363, 0x76B, 0x444, 0x054, 0xA5C, 0x66C, 0x4C5, 0x8CD, 0xADD, 0x2E5}, + {0x288, 0x690, 0x8A0, 0x0A8, 0x441, 0x651, 0xA59, 0x069, 0x4C2, 0x2CA, 0xADA, 0x8E2, + 0x34B, 0x753, 0x963, 0x16B, 0x504, 0x714, 0xB1C, 0x12C, 0x405, 0x20D, 0xA1D, 0x825}, + {0x088, 0x290, 0x6A0, 0x8A8, 0x401, 0x211, 0xA19, 0x829, 0x442, 0x04A, 0xA5A, 0x662, + 0x14B, 0x353, 0x763, 0x96B, 0x4C4, 0x2D4, 0xADC, 0x8EC, 0x505, 0x10D, 0xB1D, 0x725}, + {0x648, 0x450, 0x060, 0xA68, 0x2C1, 0x4D1, 0x8D9, 0xAE9, 0x282, 0x68A, 0x89A, 0x0A2, + 0x70B, 0x513, 0x123, 0xB2B, 0x204, 0x414, 0x81C, 0xA2C, 0x345, 0x74D, 0x95D, 0x165}, + {0xA48, 0x650, 0x460, 0x068, 0x341, 0x751, 0x959, 0x169, 0x2C2, 0xACA, 0x8DA, 0x4E2, + 0xB0B, 0x713, 0x523, 0x12B, 0x284, 0x694, 0x89C, 0x0AC, 0x205, 0xA0D, 0x81D, 0x425}, + {0x448, 0x050, 0xA60, 0x668, 0x281, 0x091, 0x899, 0x6A9, 0x202, 0x40A, 0x81A, 0xA22, + 0x50B, 0x113, 0xB23, 0x72B, 0x344, 0x154, 0x95C, 0x76C, 0x2C5, 0x4CD, 0x8DD, 0xAE5}, + {0x048, 0xA50, 0x660, 0x468, 0x201, 0xA11, 0x819, 0x429, 0x342, 0x14A, 0x95A, 0x762, + 0x10B, 0xB13, 0x723, 0x52B, 0x2C4, 0xAD4, 0x8DC, 0x4EC, 0x285, 0x08D, 0x89D, 0x6A5}, + {0x808, 0xA10, 0x220, 0x428, 0x101, 0xB11, 0x719, 0x529, 0x142, 0x94A, 0x75A, 0x362, + 0x8CB, 0xAD3, 0x2E3, 0x4EB, 0x044, 0xA54, 0x65C, 0x46C, 0x085, 0x88D, 0x69D, 0x2A5}, + {0xA08, 0x210, 0x420, 0x828, 0x141, 0x351, 0x759, 0x969, 0x042, 0xA4A, 0x65A, 0x462, + 0xACB, 0x2D3, 0x4E3, 0x8EB, 0x084, 0x294, 0x69C, 0x8AC, 0x105, 0xB0D, 0x71D, 0x525}, + {0x408, 0x810, 0xA20, 0x228, 0x081, 0x891, 0x699, 0x2A9, 0x102, 0x50A, 0x71A, 0xB22, + 0x4CB, 0x8D3, 0xAE3, 0x2EB, 0x144, 0x954, 0x75C, 0x36C, 0x045, 0x44D, 0x65D, 0xA65}, }; // _axis_convert_num = {'X': 0, 'Y': 1, 'Z': 2, '-X': 3, '-Y': 4, '-Z': 5} BLI_INLINE int _axis_signed(const int axis) { - return (axis < 3) ? axis : axis - 3; + return (axis < 3) ? axis : axis - 3; } /** @@ -2284,63 +2301,56 @@ BLI_INLINE int _axis_signed(const int axis) * where the first 2 are a source and the second 2 are the target. */ bool mat3_from_axis_conversion( - int src_forward, int src_up, int dst_forward, int dst_up, - float r_mat[3][3]) -{ - // from functools import reduce - int value; - - if (src_forward == dst_forward && src_up == dst_up) { - unit_m3(r_mat); - return false; - } - - if ((_axis_signed(src_forward) == _axis_signed(src_up)) || - (_axis_signed(dst_forward) == _axis_signed(dst_up))) - { - /* we could assert here! */ - unit_m3(r_mat); - return false; - } - - value = ((src_forward << (0 * 3)) | - (src_up << (1 * 3)) | - (dst_forward << (2 * 3)) | - (dst_up << (3 * 3))); - - for (uint i = 0; i < (sizeof(_axis_convert_matrix) / sizeof(*_axis_convert_matrix)); i++) { - for (uint j = 0; j < (sizeof(*_axis_convert_lut) / sizeof(*_axis_convert_lut[0])); j++) { - if (_axis_convert_lut[i][j] == value) { - copy_m3_m3(r_mat, _axis_convert_matrix[i]); - return true; - } - } - - } -// BLI_assert(0); - return false; + int src_forward, int src_up, int dst_forward, int dst_up, float r_mat[3][3]) +{ + // from functools import reduce + int value; + + if (src_forward == dst_forward && src_up == dst_up) { + unit_m3(r_mat); + return false; + } + + if ((_axis_signed(src_forward) == _axis_signed(src_up)) || + (_axis_signed(dst_forward) == _axis_signed(dst_up))) { + /* we could assert here! */ + unit_m3(r_mat); + return false; + } + + value = ((src_forward << (0 * 3)) | (src_up << (1 * 3)) | (dst_forward << (2 * 3)) | + (dst_up << (3 * 3))); + + for (uint i = 0; i < (sizeof(_axis_convert_matrix) / sizeof(*_axis_convert_matrix)); i++) { + for (uint j = 0; j < (sizeof(*_axis_convert_lut) / sizeof(*_axis_convert_lut[0])); j++) { + if (_axis_convert_lut[i][j] == value) { + copy_m3_m3(r_mat, _axis_convert_matrix[i]); + return true; + } + } + } + // BLI_assert(0); + return false; } /** * Use when the second axis can be guessed. */ -bool mat3_from_axis_conversion_single( - int src_axis, int dst_axis, - float r_mat[3][3]) +bool mat3_from_axis_conversion_single(int src_axis, int dst_axis, float r_mat[3][3]) { - if (src_axis == dst_axis) { - unit_m3(r_mat); - return false; - } + if (src_axis == dst_axis) { + unit_m3(r_mat); + return false; + } - /* Pick predictable next axis. */ - int src_axis_next = (src_axis + 1) % 3; - int dst_axis_next = (dst_axis + 1) % 3; + /* Pick predictable next axis. */ + int src_axis_next = (src_axis + 1) % 3; + int dst_axis_next = (dst_axis + 1) % 3; - if ((src_axis < 3) != (dst_axis < 3)) { - /* Flip both axis so matrix sign remains positive. */ - dst_axis_next += 3; - } + if ((src_axis < 3) != (dst_axis < 3)) { + /* Flip both axis so matrix sign remains positive. */ + dst_axis_next += 3; + } - return mat3_from_axis_conversion(src_axis, src_axis_next, dst_axis, dst_axis_next, r_mat); + return mat3_from_axis_conversion(src_axis, src_axis_next, dst_axis, dst_axis_next, r_mat); } |