1 files changed, 33 insertions, 5 deletions

diff --git a/source/blender/blenlib/intern/math_rotation.c b/source/blender/blenlib/intern/math_rotation.c
index 5f6f7ac54d9..a3e57605ad4 100644
--- a/source/blender/blenlib/intern/math_rotation.c
+++ b/source/blender/blenlib/intern/math_rotation.c
@@ -31,6 +31,9 @@
 
 /******************************** Quaternions ********************************/
 
+/* used to test is a quat is not normalized */
+#define QUAT_EPSILON 0.00001
+
 void unit_qt(float *q)
 {
 	q[0]= 1.0f;
@@ -144,7 +147,8 @@ void mul_fac_qt_fl(float *q, const float fac)
 	mul_v3_fl(q+1, si);
 }
 
-void quat_to_mat3(float m[][3], const float q[4])
+/* skip error check, currently only needed by mat3_to_quat_is_ok */
+static void quat_to_mat3_no_assert(float m[][3], const float q[4])
 {
 	double q0, q1, q2, q3, qda,qdb,qdc,qaa,qab,qac,qbb,qbc,qcc;
 
@@ -176,10 +180,23 @@ void quat_to_mat3(float m[][3], const float q[4])
 	m[2][2]= (float)(1.0-qaa-qbb);
 }
 
+
+void quat_to_mat3(float m[][3], const float q[4])
+{
+	/* throw an error if the quat isn't normalized */
+	float f;
+	assert((f=dot_qtqt(q, q))==0.0 || (fabs(f-1.0) < QUAT_EPSILON));
+
+	quat_to_mat3_no_assert(m, q);
+}
+
 void quat_to_mat4(float m[][4], const float q[4])
 {
 	double q0, q1, q2, q3, qda,qdb,qdc,qaa,qab,qac,qbb,qbc,qcc;
 
+	/* throw an error if the quat isn't normalized */
+	assert((q0=dot_qtqt(q, q))==0.0 || (fabs(q0-1.0) < QUAT_EPSILON));
+
 	q0= M_SQRT2 * q[0];
 	q1= M_SQRT2 * q[1];
 	q2= M_SQRT2 * q[2];
@@ -300,7 +317,7 @@ void mat3_to_quat_is_ok(float q[4], float wmat[3][3])
 	q1[3]= -nor[2]*si;
 
 	/* rotate back x-axis from mat, using inverse q1 */
-	quat_to_mat3( matr,q1);
+	quat_to_mat3_no_assert( matr,q1);
 	invert_m3_m3(matn, matr);
 	mul_m3_v3(matn, mat[0]);
 	
@@ -318,7 +335,7 @@ void mat3_to_quat_is_ok(float q[4], float wmat[3][3])
 }
 
 
-void normalize_qt(float *q)
+float normalize_qt(float *q)
 {
 	float len;
 	
@@ -330,6 +347,14 @@ void normalize_qt(float *q)
 		q[1]= 1.0f;
 		q[0]= q[2]= q[3]= 0.0f;			
 	}
+
+	return len;
+}
+
+float normalize_qt_qt(float r[4], const float q[4])
+{
+	copy_qt_qt(r, q);
+	return normalize_qt(r);
 }
 
 /* note: expects vectors to be normalized */
@@ -619,7 +644,10 @@ void axis_angle_to_quat(float q[4], const float axis[3], float angle)
 void quat_to_axis_angle(float axis[3], float *angle, const float q[4])
 {
 	float ha, si;
-	
+
+	/* throw an error if the quat isn't normalized */
+	assert((ha=dot_qtqt(q, q))==0.0 || (fabs(ha-1.0) < QUAT_EPSILON));
+
 	/* calculate angle/2, and sin(angle/2) */
 	ha= (float)acos(q[0]);
 	si= (float)sin(ha);
@@ -925,7 +953,7 @@ void mat4_to_eul(float *eul,float tmat[][4])
 void quat_to_eul(float *eul, const float quat[4])
 {
 	float mat[3][3];
-	
+
 	quat_to_mat3(mat,quat);
 	mat3_to_eul(eul,mat);
 }