diff options
| -rw-r--r-- | source/blender/blenlib/BLI_math_base.h | 7 | ||||
| -rw-r--r-- | source/blender/blenlib/BLI_math_rotation.h | 5 | ||||
| -rw-r--r-- | source/blender/blenlib/intern/math_rotation.c | 38 |
3 files changed, 50 insertions, 0 deletions
diff --git a/source/blender/blenlib/BLI_math_base.h b/source/blender/blenlib/BLI_math_base.h index ef05777dc2f..63a9bac288e 100644 --- a/source/blender/blenlib/BLI_math_base.h +++ b/source/blender/blenlib/BLI_math_base.h @@ -269,6 +269,12 @@ double double_round(double x, int ndigits); (fabsf(_test_unit) < BLI_ASSERT_UNIT_EPSILON)); \ } (void)0 +# define BLI_ASSERT_UNIT_QUAT(q) { \ + const float _test_unit = dot_qtqt(q, q); \ + BLI_assert((fabsf(_test_unit - 1.0f) < BLI_ASSERT_UNIT_EPSILON) || \ + (fabsf(_test_unit) < BLI_ASSERT_UNIT_EPSILON)); \ +} (void)0 + # define BLI_ASSERT_ZERO_M3(m) { \ BLI_assert(dot_vn_vn((const float *)m, (const float *)m, 9) != 0.0); \ } (void)0 @@ -279,6 +285,7 @@ double double_round(double x, int ndigits); #else # define BLI_ASSERT_UNIT_V2(v) (void)(v) # define BLI_ASSERT_UNIT_V3(v) (void)(v) +# define BLI_ASSERT_UNIT_QUAT(v) (void)(v) # define BLI_ASSERT_ZERO_M3(m) (void)(m) # define BLI_ASSERT_ZERO_M4(m) (void)(m) #endif diff --git a/source/blender/blenlib/BLI_math_rotation.h b/source/blender/blenlib/BLI_math_rotation.h index 54bfef4bd14..477b3280d09 100644 --- a/source/blender/blenlib/BLI_math_rotation.h +++ b/source/blender/blenlib/BLI_math_rotation.h @@ -86,6 +86,11 @@ void vec_to_quat(float q[4], const float vec[3], short axis, const short upflag void rotation_between_vecs_to_quat(float q[4], const float v1[3], const float v2[3]); void rotation_between_quats_to_quat(float q[4], const float q1[4], const float q2[4]); +float angle_normalized_qt(const float q[4]); +float angle_normalized_qtqt(const float q1[4], const float q2[4]); +float angle_qt(const float q[4]); +float angle_qtqt(const float q1[4], const float q2[4]); + /* TODO: don't what this is, but it's not the same as mat3_to_quat */ void mat3_to_quat_is_ok(float q[4], float mat[3][3]); diff --git a/source/blender/blenlib/intern/math_rotation.c b/source/blender/blenlib/intern/math_rotation.c index d1753a77f7f..d7daa23c087 100644 --- a/source/blender/blenlib/intern/math_rotation.c +++ b/source/blender/blenlib/intern/math_rotation.c @@ -440,6 +440,44 @@ void rotation_between_quats_to_quat(float q[4], const float q1[4], const float q mul_qt_qtqt(q, tquat, q2); } + +float angle_normalized_qt(const float q[4]) +{ + BLI_ASSERT_UNIT_QUAT(q); + return 2.0f * saacos(q[0]); +} + +float angle_qt(const float q[4]) +{ + float tquat[4]; + + normalize_qt_qt(tquat, q); + + return angle_normalized_qt(tquat); +} + +float angle_normalized_qtqt(const float q1[4], const float q2[4]) +{ + float qdelta[4]; + + BLI_ASSERT_UNIT_QUAT(q1); + BLI_ASSERT_UNIT_QUAT(q2); + + rotation_between_quats_to_quat(qdelta, q1, q2); + + return angle_normalized_qt(qdelta); +} + +float angle_qtqt(const float q1[4], const float q2[4]) +{ + float quat1[4], quat2[4]; + + normalize_qt_qt(quat1, q1); + normalize_qt_qt(quat2, q2); + + return angle_normalized_qtqt(quat1, quat2); +} + void vec_to_quat(float q[4], const float vec[3], short axis, const short upflag) { float nor[3], tvec[3]; |