Added an assert() check for normalized quats which exposed a number of bugs where normalized quat was incorrectly assumed.

This would have made bug #25003 very simple to find.

- Objects had their quats normalized when calculating their matrix, this is inconstant with pose bones and isn't useful for animation.
  Also it wasn't normalizing the delta rotation so these would give bad rotations.

- Converting between rotation modes BKE_rotMode_change_values() assumed normal length quat. changing quat to euler rotation for eg could change the bone.

- Clear rotation and transform were not normalizing the quat when 4d loc was disabled on quat rotation, corrected and also made it so the quat scale is restored after conversion so animations curves dont jump.

There is 1 case in mat3_to_quat_is_ok() where quat_to_mat3 on an unnormalized quat is needed, for this I had to add an ugly static function quat_to_mat3_no_assert(), but overall its worthwhile IMHO to be able to find incorrect use of rotation conversion.

7 files changed, 81 insertions, 34 deletions

diff --git a/source/blender/blenkernel/intern/anim.c b/source/blender/blenkernel/intern/anim.c
index b58bd8c7f3e..b479bd5ef28 100644
--- a/source/blender/blenkernel/intern/anim.c
+++ b/source/blender/blenkernel/intern/anim.c
@@ -1339,12 +1339,16 @@ static void new_particle_duplilist(ListBase *lb, ID *UNUSED(id), Scene *scene, O
 			else {
 				/* first key */
 				state.time = ctime;
-				if(psys_get_particle_state(&sim, a, &state, 0) == 0)
+				if(psys_get_particle_state(&sim, a, &state, 0) == 0) {
 					continue;
-
-				quat_to_mat4( pamat,state.rot);
-				VECCOPY(pamat[3], state.co);
-				pamat[3][3]= 1.0f;
+				}
+				else {
+					float tquat[4];
+					normalize_qt_qt(tquat, state.rot);
+					quat_to_mat4(pamat, tquat);
+					copy_v3_v3(pamat[3], state.co);
+					pamat[3][3]= 1.0f;
+				}
 			}
 
 			if(part->ren_as==PART_DRAW_GR && psys->part->draw & PART_DRAW_WHOLE_GR) {
diff --git a/source/blender/blenkernel/intern/armature.c b/source/blender/blenkernel/intern/armature.c
index b4bdb516ab3..0d1d08af44c 100644
--- a/source/blender/blenkernel/intern/armature.c
+++ b/source/blender/blenkernel/intern/armature.c
@@ -1248,6 +1248,7 @@ void BKE_rotMode_change_values (float quat[4], float eul[3], float axis[3], floa
 		}
 		else if (oldMode == ROT_MODE_QUAT) {
 			/* quat to euler */
+			normalize_qt(quat);
 			quat_to_eulO( eul, newMode,quat);
 		}
 		/* else { no conversion needed } */
@@ -1270,6 +1271,7 @@ void BKE_rotMode_change_values (float quat[4], float eul[3], float axis[3], floa
 		}
 		else if (oldMode == ROT_MODE_QUAT) {
 			/* quat to axis angle */
+			normalize_qt(quat);
 			quat_to_axis_angle( axis, angle,quat);
 		}
 		
@@ -2092,8 +2094,7 @@ void pchan_to_mat4(bPoseChannel *pchan, float chan_mat[4][4])
 		 * but if this proves to be too problematic, switch back to the old system of operating directly on 
 		 * the stored copy
 		 */
-		QUATCOPY(quat, pchan->quat);
-		normalize_qt(quat);
+		normalize_qt_qt(quat, pchan->quat);
 		quat_to_mat3(rmat, quat);
 	}
 	
diff --git a/source/blender/blenkernel/intern/object.c b/source/blender/blenkernel/intern/object.c
index 5f2a10c0b3e..b98927db877 100644
--- a/source/blender/blenkernel/intern/object.c
+++ b/source/blender/blenkernel/intern/object.c
@@ -1668,9 +1668,13 @@ void object_rot_to_mat3(Object *ob, float mat[][3])
 	}
 	else {
 		/* quats are normalised before use to eliminate scaling issues */
-		normalize_qt(ob->quat);
-		quat_to_mat3( rmat,ob->quat);
-		quat_to_mat3( dmat,ob->dquat);
+		float tquat[4];
+
+		normalize_qt_qt(tquat, ob->quat);
+		quat_to_mat3(rmat, tquat);
+
+		normalize_qt_qt(tquat, ob->quat);
+		quat_to_mat3(dmat, tquat);
 	}
 	
 	/* combine these rotations */
@@ -1818,8 +1822,8 @@ static void ob_parcurve(Scene *scene, Object *ob, Object *par, float mat[][4])
 #else
 			quat_apply_track(quat, ob->trackflag, ob->upflag);
 #endif
-
-			quat_to_mat4(mat,quat);			
+			normalize_qt(quat);
+			quat_to_mat4(mat, quat);
 		}
 		
 		if(cu->flag & CU_PATH_RADIUS) {
diff --git a/source/blender/blenlib/BLI_math_rotation.h b/source/blender/blenlib/BLI_math_rotation.h
index 19ecce1040b..72147962316 100644
--- a/source/blender/blenlib/BLI_math_rotation.h
+++ b/source/blender/blenlib/BLI_math_rotation.h
@@ -51,10 +51,11 @@ void mul_fac_qt_fl(float q[4], const float f);
 void sub_qt_qtqt(float q[4], const float a[4], const float b[4]);
 
 void invert_qt(float q[4]);
-void invert_qt_qt(float *q1, const float *q2);
+void invert_qt_qt(float q1[4], const float q2[4]);
 void conjugate_qt(float q[4]);
 float dot_qtqt(const float a[4], const float b[4]);
-void normalize_qt(float q[4]);
+float normalize_qt(float q[4]);
+float normalize_qt_qt(float q1[4], const float q2[4]);
 
 /* comparison */
 int is_zero_qt(float q[4]);
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);
 }
diff --git a/source/blender/editors/armature/editarmature.c b/source/blender/editors/armature/editarmature.c
index 2e251a67a34..36544bcb086 100644
--- a/source/blender/editors/armature/editarmature.c
+++ b/source/blender/editors/armature/editarmature.c
@@ -5026,10 +5026,11 @@ static int pose_clear_rot_exec(bContext *C, wmOperator *UNUSED(op))
 			else {
 				/* perform clamping using euler form (3-components) */
 				float eul[3], oldeul[3], quat1[4] = {0};
+				float qlen;
 				
 				if (pchan->rotmode == ROT_MODE_QUAT) {
-					copy_qt_qt(quat1, pchan->quat);
-					quat_to_eul( oldeul,pchan->quat);
+					qlen= normalize_qt_qt(quat1, pchan->quat);
+					quat_to_eul(oldeul, quat1);
 				}
 				else if (pchan->rotmode == ROT_MODE_AXISANGLE) {
 					axis_angle_to_eulO( oldeul, EULER_ORDER_DEFAULT,pchan->rotAxis, pchan->rotAngle);
@@ -5048,7 +5049,11 @@ static int pose_clear_rot_exec(bContext *C, wmOperator *UNUSED(op))
 					eul[2]= oldeul[2];
 				
 				if (pchan->rotmode == ROT_MODE_QUAT) {
-					eul_to_quat( pchan->quat,eul);
+					eul_to_quat(pchan->quat, eul);
+
+					/* restore original quat size */
+					mul_qt_fl(pchan->quat, qlen);
+
 					/* quaternions flip w sign to accumulate rotations correctly */
 					if ((quat1[0]<0.0f && pchan->quat[0]>0.0f) || (quat1[0]>0.0f && pchan->quat[0]<0.0f)) {
 						mul_qt_fl(pchan->quat, -1.0f);
@@ -5064,8 +5069,7 @@ static int pose_clear_rot_exec(bContext *C, wmOperator *UNUSED(op))
 		}						// Duplicated in source/blender/editors/object/object_transform.c
 		else { 
 			if (pchan->rotmode == ROT_MODE_QUAT) {
-				pchan->quat[1]=pchan->quat[2]=pchan->quat[3]= 0.0f; 
-				pchan->quat[0]= 1.0f;
+				unit_qt(pchan->quat);
 			}
 			else if (pchan->rotmode == ROT_MODE_AXISANGLE) {
 				/* by default, make rotation of 0 radians around y-axis (roll) */
diff --git a/source/blender/editors/transform/transform.c b/source/blender/editors/transform/transform.c
index 36438712d09..4b6079001ff 100644
--- a/source/blender/editors/transform/transform.c
+++ b/source/blender/editors/transform/transform.c
@@ -1917,23 +1917,29 @@ static void protectedQuaternionBits(short protectflag, float *quat, float *oldqu
 	}
 	else {
 		/* quaternions get limited with euler... (compatability mode) */
-		float eul[3], oldeul[3], quat1[4];
-		
-		QUATCOPY(quat1, quat);
-		quat_to_eul( eul,quat);
-		quat_to_eul( oldeul,oldquat);
-		
+		float eul[3], oldeul[3], nquat[4], noldquat[4];
+		float qlen;
+
+		qlen= normalize_qt_qt(nquat, quat);
+		normalize_qt_qt(noldquat, oldquat);
+
+		quat_to_eul(eul, nquat);
+		quat_to_eul(oldeul, noldquat);
+
 		if (protectflag & OB_LOCK_ROTX)
 			eul[0]= oldeul[0];
 		if (protectflag & OB_LOCK_ROTY)
 			eul[1]= oldeul[1];
 		if (protectflag & OB_LOCK_ROTZ)
 			eul[2]= oldeul[2];
-		
+
 		eul_to_quat( quat,eul);
+
+		/* restore original quat size */
+		mul_qt_fl(quat, qlen);
 		
 		/* quaternions flip w sign to accumulate rotations correctly */
-		if ( (quat1[0]<0.0f && quat[0]>0.0f) || (quat1[0]>0.0f && quat[0]<0.0f) ) {
+		if ( (nquat[0]<0.0f && quat[0]>0.0f) || (nquat[0]>0.0f && quat[0]<0.0f) ) {
 			mul_qt_fl(quat, -1.0f);
 		}
 	}
@@ -2013,8 +2019,7 @@ static void constraintob_from_transdata(bConstraintOb *cob, TransData *td)
 			   we don't necessarily end up with a rotation matrix, and
 			   then conversion back to quat gives a different result */
 			float quat[4];
-			copy_qt_qt(quat, td->ext->quat);
-			normalize_qt(quat);
+			normalize_qt_qt(quat, td->ext->quat);
 			quat_to_mat4(cob->matrix, quat);
 		}
 		else if (td->ext->rotOrder == ROT_MODE_AXISANGLE) {