[Bf-blender-cvs] [c6252d6] master: Math Lib: add angle_qt functions

[Campbell Barton]

Commit: c6252d6e608a0dff72a8d5fde404fa6bd2c7ca7b
Author: Campbell Barton
Date:   Thu Mar 20 14:39:32 2014 +1100
https://developer.blender.org/rBc6252d6e608a0dff72a8d5fde404fa6bd2c7ca7b

Math Lib: add angle_qt functions

===================================================================

M	source/blender/blenlib/BLI_math_base.h
M	source/blender/blenlib/BLI_math_rotation.h
M	source/blender/blenlib/intern/math_rotation.c

===================================================================

diff --git a/source/blender/blenlib/BLI_math_base.h b/source/blender/blenlib/BLI_math_base.h
index ef05777..63a9bac 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 54bfef4..477b328 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 d1753a7..d7daa23 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];