BLI_math: add mat3_normalized_to_* functions

Many uses of matrices for rotation keep them normalized,
so no need to normalize each time.

10 files changed, 198 insertions, 101 deletions

diff --git a/source/blender/blenkernel/intern/armature.c b/source/blender/blenkernel/intern/armature.c
index 6afe7f1abe9..fde25784c22 100644
--- a/source/blender/blenkernel/intern/armature.c
+++ b/source/blender/blenkernel/intern/armature.c
@@ -1325,18 +1325,20 @@ void BKE_armature_mat_pose_to_bone_ex(Object *ob, bPoseChannel *pchan, float inm
 /* same as BKE_object_mat3_to_rot() */
 void BKE_pchan_mat3_to_rot(bPoseChannel *pchan, float mat[3][3], bool use_compat)
 {
+	BLI_ASSERT_UNIT_M3(mat);
+
 	switch (pchan->rotmode) {
 		case ROT_MODE_QUAT:
-			mat3_to_quat(pchan->quat, mat);
+			mat3_normalized_to_quat(pchan->quat, mat);
 			break;
 		case ROT_MODE_AXISANGLE:
-			mat3_to_axis_angle(pchan->rotAxis, &pchan->rotAngle, mat);
+			mat3_normalized_to_axis_angle(pchan->rotAxis, &pchan->rotAngle, mat);
 			break;
 		default: /* euler */
 			if (use_compat)
-				mat3_to_compatible_eulO(pchan->eul, pchan->eul, pchan->rotmode, mat);
+				mat3_normalized_to_compatible_eulO(pchan->eul, pchan->eul, pchan->rotmode, mat);
 			else
-				mat3_to_eulO(pchan->eul, pchan->rotmode, mat);
+				mat3_normalized_to_eulO(pchan->eul, pchan->rotmode, mat);
 			break;
 	}
 }
diff --git a/source/blender/blenkernel/intern/constraint.c b/source/blender/blenkernel/intern/constraint.c
index 03406c6c2a0..906a9e4e484 100644
--- a/source/blender/blenkernel/intern/constraint.c
+++ b/source/blender/blenkernel/intern/constraint.c
@@ -3863,7 +3863,7 @@ static void pivotcon_evaluate(bConstraint *con, bConstraintOb *cob, ListBase *ta
 
 
 	/* correct the pivot by the rotation axis otherwise the pivot translates when it shouldnt */
-	mat3_to_axis_angle(axis, &angle, rotMat);
+	mat3_normalized_to_axis_angle(axis, &angle, rotMat);
 	if (angle) {
 		float dvec[3];
 		sub_v3_v3v3(vec, pivot, cob->matrix[3]);
diff --git a/source/blender/blenkernel/intern/object.c b/source/blender/blenkernel/intern/object.c
index 61c4073a2fd..4ec41373116 100644
--- a/source/blender/blenkernel/intern/object.c
+++ b/source/blender/blenkernel/intern/object.c
@@ -1945,11 +1945,13 @@ void BKE_object_rot_to_mat3(Object *ob, float mat[3][3], bool use_drot)
 
 void BKE_object_mat3_to_rot(Object *ob, float mat[3][3], bool use_compat)
 {
+	BLI_ASSERT_UNIT_M3(mat);
+
 	switch (ob->rotmode) {
 		case ROT_MODE_QUAT:
 		{
 			float dquat[4];
-			mat3_to_quat(ob->quat, mat);
+			mat3_normalized_to_quat(ob->quat, mat);
 			normalize_qt_qt(dquat, ob->dquat);
 			invert_qt_normalized(dquat);
 			mul_qt_qtqt(ob->quat, dquat, ob->quat);
@@ -1961,7 +1963,7 @@ void BKE_object_mat3_to_rot(Object *ob, float mat[3][3], bool use_compat)
 			float dquat[4];
 
 			/* without drot we could apply 'mat' directly */
-			mat3_to_quat(quat, mat);
+			mat3_normalized_to_quat(quat, mat);
 			axis_angle_to_quat(dquat, ob->drotAxis, ob->drotAngle);
 			invert_qt_normalized(dquat);
 			mul_qt_qtqt(quat, dquat, quat);
@@ -1972,18 +1974,16 @@ void BKE_object_mat3_to_rot(Object *ob, float mat[3][3], bool use_compat)
 		{
 			float quat[4];
 			float dquat[4];
-			float tmat[3][3];
 
 			/* without drot we could apply 'mat' directly */
-			mat3_to_quat(quat, mat);
+			mat3_normalized_to_quat(quat, mat);
 			eulO_to_quat(dquat, ob->drot, ob->rotmode);
 			invert_qt_normalized(dquat);
 			mul_qt_qtqt(quat, dquat, quat);
-			quat_to_mat3(tmat, quat);
 			/* end drot correction */
 
-			if (use_compat) mat3_to_compatible_eulO(ob->rot, ob->rot, ob->rotmode, tmat);
-			else            mat3_to_eulO(ob->rot, ob->rotmode, tmat);
+			if (use_compat) quat_to_compatible_eulO(ob->rot, ob->rot, ob->rotmode, quat);
+			else            quat_to_eulO(ob->rot, ob->rotmode, quat);
 			break;
 		}
 	}
diff --git a/source/blender/blenlib/BLI_math_rotation.h b/source/blender/blenlib/BLI_math_rotation.h
index 4c4e305c965..24c20ee7b50 100644
--- a/source/blender/blenlib/BLI_math_rotation.h
+++ b/source/blender/blenlib/BLI_math_rotation.h
@@ -79,6 +79,8 @@ void add_qt_qtqt(float q[4], const float a[4], const float b[4], const float t);
 void quat_to_mat3(float mat[3][3], const float q[4]);
 void quat_to_mat4(float mat[4][4], const float q[4]);
 
+void mat3_normalized_to_quat(float q[4], float mat[3][3]);
+void mat4_normalized_to_quat(float q[4], float mat[4][4]);
 void mat3_to_quat(float q[4], float mat[3][3]);
 void mat4_to_quat(float q[4], float mat[4][4]);
 void tri_to_quat_ex(float quat[4], const float v1[3], const float v2[3], const float v3[3],
@@ -114,9 +116,11 @@ void axis_angle_normalized_to_mat3_ex(float mat[3][3], const float axis[3],
 void axis_angle_normalized_to_mat3(float R[3][3], const float axis[3], const float angle);
 void axis_angle_to_mat4(float R[4][4], const float axis[3], const float angle);
 
-void quat_to_axis_angle(float axis[3], float *angle, const float q[4]);
+void mat3_normalized_to_axis_angle(float axis[3], float *angle, float M[3][3]);
+void mat4_normalized_to_axis_angle(float axis[3], float *angle, float M[4][4]);
 void mat3_to_axis_angle(float axis[3], float *angle, float M[3][3]);
 void mat4_to_axis_angle(float axis[3], float *angle, float M[4][4]);
+void quat_to_axis_angle(float axis[3], float *angle, const float q[4]);
 
 void axis_angle_to_mat3_single(float R[3][3], const char axis, const float angle);
 void      angle_to_mat2(float R[2][2], const float angle);
@@ -134,12 +138,16 @@ void eul_to_quat(float quat[4], const float eul[3]);
 void eul_to_mat3(float mat[3][3], const float eul[3]);
 void eul_to_mat4(float mat[4][4], const float eul[3]);
 
-void quat_to_eul(float eul[3], const float quat[4]);
+void mat3_normalized_to_eul(float eul[3], float mat[3][3]);
+void mat4_normalized_to_eul(float eul[3], float mat[4][4]);
 void mat3_to_eul(float eul[3], float mat[3][3]);
 void mat4_to_eul(float eul[3], float mat[4][4]);
+void quat_to_eul(float eul[3], const float quat[4]);
 
-void compatible_eul(float eul[3], const float old[3]);
+void mat3_normalized_to_compatible_eul(float eul[3], const float old[3], float mat[3][3]);
 void mat3_to_compatible_eul(float eul[3], const float old[3], float mat[3][3]);
+void quat_to_compatible_eul(float eul[3], const float oldrot[3], const float quat[4]);
+void compatible_eul(float eul[3], const float old[3]);
 
 void rotate_eul(float eul[3], const char axis, const float angle);
 
@@ -164,14 +172,19 @@ void eulO_to_mat3(float mat[3][3], const float eul[3], const short order);
 void eulO_to_mat4(float mat[4][4], const float eul[3], const short order);
 void eulO_to_axis_angle(float axis[3], float *angle, const float eul[3], const short order);
 void eulO_to_gimbal_axis(float gmat[3][3], const float eul[3], const short order);
- 
-void quat_to_eulO(float eul[3], const short order, const float quat[4]);
+
+void mat3_normalized_to_eulO(float eul[3], const short order, float mat[3][3]);
+void mat4_normalized_to_eulO(float eul[3], const short order, float mat[4][4]);
 void mat3_to_eulO(float eul[3], const short order, float mat[3][3]);
 void mat4_to_eulO(float eul[3], const short order, float mat[4][4]);
+void quat_to_eulO(float eul[3], const short order, const float quat[4]);
 void axis_angle_to_eulO(float eul[3], const short order, const float axis[3], const float angle);
 
+void mat3_normalized_to_compatible_eulO(float eul[3], float old[3], const short order, float mat[3][3]);
+void mat4_normalized_to_compatible_eulO(float eul[3], float old[3], const short order, float mat[4][4]);
 void mat3_to_compatible_eulO(float eul[3], float old[3], const short order, float mat[3][3]);
 void mat4_to_compatible_eulO(float eul[3], float old[3], const short order, float mat[4][4]);
+void quat_to_compatible_eulO(float eul[3], float old[3], const short order, const float quat[4]);
 
 void rotate_eulO(float eul[3], const short order, char axis, float angle);
 
diff --git a/source/blender/blenlib/intern/math_matrix.c b/source/blender/blenlib/intern/math_matrix.c
index 4bf6d162970..19d116928fd 100644
--- a/source/blender/blenlib/intern/math_matrix.c
+++ b/source/blender/blenlib/intern/math_matrix.c
@@ -1549,7 +1549,7 @@ void mat4_to_loc_quat(float loc[3], float quat[4], float wmat[4][4])
 		negate_m3(mat3_n);
 	}
 
-	mat3_to_quat(quat, mat3_n);
+	mat3_normalized_to_quat(quat, mat3_n);
 	copy_v3_v3(loc, wmat[3]);
 }
 
@@ -1557,7 +1557,7 @@ void mat4_decompose(float loc[3], float quat[4], float size[3], float wmat[4][4]
 {
 	float rot[3][3];
 	mat4_to_loc_rot_size(loc, rot, size, wmat);
-	mat3_to_quat(quat, rot);
+	mat3_normalized_to_quat(quat, rot);
 }
 
 /**
@@ -1692,8 +1692,8 @@ void blend_m3_m3m3(float out[3][3], float dst[3][3], float src[3][3], const floa
 	mat3_to_rot_size(drot, dscale, dst);
 	mat3_to_rot_size(srot, sscale, src);
 
-	mat3_to_quat(dquat, drot);
-	mat3_to_quat(squat, srot);
+	mat3_normalized_to_quat(dquat, drot);
+	mat3_normalized_to_quat(squat, srot);
 
 	/* do blending */
 	interp_qt_qtqt(fquat, dquat, squat, srcweight);
@@ -1715,8 +1715,8 @@ void blend_m4_m4m4(float out[4][4], float dst[4][4], float src[4][4], const floa
 	mat4_to_loc_rot_size(dloc, drot, dscale, dst);
 	mat4_to_loc_rot_size(sloc, srot, sscale, src);
 
-	mat3_to_quat(dquat, drot);
-	mat3_to_quat(squat, srot);
+	mat3_normalized_to_quat(dquat, drot);
+	mat3_normalized_to_quat(squat, srot);
 
 	/* do blending */
 	interp_v3_v3v3(floc, dloc, sloc, srcweight);
diff --git a/source/blender/blenlib/intern/math_rotation.c b/source/blender/blenlib/intern/math_rotation.c
index 88c577f3517..f89f6225a90 100644
--- a/source/blender/blenlib/intern/math_rotation.c
+++ b/source/blender/blenlib/intern/math_rotation.c
@@ -293,14 +293,11 @@ void quat_to_mat4(float m[4][4], const float q[4])
 	m[3][3] = 1.0f;
 }
 
-void mat3_to_quat(float q[4], float wmat[3][3])
+void mat3_normalized_to_quat(float q[4], float mat[3][3])
 {
 	double tr, s;
-	float mat[3][3];
 
-	/* work on a copy */
-	copy_m3_m3(mat, wmat);
-	normalize_m3(mat); /* this is needed AND a 'normalize_qt' in the end */
+	BLI_ASSERT_UNIT_M3(mat);
 
 	tr = 0.25 * (double)(1.0f + mat[0][0] + mat[1][1] + mat[2][2]);
 
@@ -344,13 +341,30 @@ void mat3_to_quat(float q[4], float wmat[3][3])
 
 	normalize_qt(q);
 }
+void mat3_to_quat(float q[4], float m[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);
+}
+
+void mat4_normalized_to_quat(float q[4], float m[4][4])
+{
+	float mat3[3][3];
+
+	copy_m3_m4(mat3, m);
+	mat3_normalized_to_quat(q, mat3);
+}
 
 void mat4_to_quat(float q[4], float m[4][4])
 {
-	float mat[3][3];
+	float mat3[3][3];
 
-	copy_m3_m4(mat, m);
-	mat3_to_quat(q, mat);
+	copy_m3_m4(mat3, m);
+	mat3_to_quat(q, mat3);
 }
 
 void mat3_to_quat_is_ok(float q[4], float wmat[3][3])
@@ -954,7 +968,16 @@ void axis_angle_to_mat4(float mat[4][4], const float axis[3], const float angle)
 	copy_m4_m3(mat, tmat);
 }
 
-/* 3x3 matrix to axis angle (see Mat4ToVecRot too) */
+/* 3x3 matrix to axis angle */
+void mat3_normalized_to_axis_angle(float axis[3], float *angle, float mat[3][3])
+{
+	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);
+}
 void mat3_to_axis_angle(float axis[3], float *angle, float mat[3][3])
 {
 	float q[4];
@@ -965,7 +988,18 @@ void mat3_to_axis_angle(float axis[3], float *angle, float mat[3][3])
 	quat_to_axis_angle(axis, angle, q);
 }
 
-/* 4x4 matrix to axis angle (see Mat4ToVecRot too) */
+/* 4x4 matrix to axis angle */
+void mat4_normalized_to_axis_angle(float axis[3], float *angle, float mat[4][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);
+}
+
+/* 4x4 matrix to axis angle */
 void mat4_to_axis_angle(float axis[3], float *angle, float mat[4][4])
 {
 	float q[4];
@@ -1147,13 +1181,11 @@ void eul_to_mat4(float mat[4][4], const float eul[3])
 /* returns two euler calculation methods, so we can pick the best */
 
 /* XYZ order */
-static void mat3_to_eul2(float tmat[3][3], float eul1[3], float eul2[3])
+static void mat3_normalized_to_eul2(const float mat[3][3], float eul1[3], float eul2[3])
 {
-	float cy, mat[3][3];
-
-	normalize_m3_m3(mat, tmat);
+	const float cy = hypotf(mat[0][0], mat[0][1]);
 
-	cy = hypotf(mat[0][0], mat[0][1]);
+	BLI_ASSERT_UNIT_M3(mat);
 
 	if (cy > 16.0f * FLT_EPSILON) {
 
@@ -1176,11 +1208,11 @@ static void mat3_to_eul2(float tmat[3][3], float eul1[3], float eul2[3])
 }
 
 /* XYZ order */
-void mat3_to_eul(float *eul, float tmat[3][3])
+void mat3_normalized_to_eul(float eul[3], float mat[3][3])
 {
 	float eul1[3], eul2[3];
 
-	mat3_to_eul2(tmat, 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])) {
@@ -1190,24 +1222,33 @@ void mat3_to_eul(float *eul, float tmat[3][3])
 		copy_v3_v3(eul, eul1);
 	}
 }
+void mat3_to_eul(float eul[3], float mat[3][3])
+{
+	float tmat[3][3];
+	normalize_m3_m3(tmat, mat);
+	mat3_normalized_to_eul(eul, mat);
+}
 
 /* XYZ order */
-void mat4_to_eul(float *eul, float tmat[4][4])
+void mat4_normalized_to_eul(float eul[3], float m[4][4])
 {
-	float tempMat[3][3];
-
-	copy_m3_m4(tempMat, tmat);
-	normalize_m3(tempMat);
-	mat3_to_eul(eul, tempMat);
+	float mat3[3][3];
+	copy_m3_m4(mat3, m);
+	mat3_normalized_to_eul(eul, mat3);
+}
+void mat4_to_eul(float eul[3], float m[4][4])
+{
+	float mat3[3][3];
+	copy_m3_m4(mat3, m);
+	mat3_to_eul(eul, mat3);
 }
 
 /* XYZ order */
-void quat_to_eul(float *eul, const float quat[4])
+void quat_to_eul(float eul[3], const float quat[4])
 {
-	float mat[3][3];
-
-	quat_to_mat3(mat, quat);
-	mat3_to_eul(eul, mat);
+	float unit_mat[3][3];
+	quat_to_mat3(unit_mat, quat);
+	mat3_normalized_to_eul(eul, unit_mat);
 }
 
 /* XYZ order */
@@ -1297,12 +1338,12 @@ void compatible_eul(float eul[3], const float oldrot[3])
 /* uses 2 methods to retrieve eulers, and picks the closest */
 
 /* XYZ order */
-void mat3_to_compatible_eul(float eul[3], const float oldrot[3], float mat[3][3])
+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;
 
-	mat3_to_eul2(mat, eul1, eul2);
+	mat3_normalized_to_eul2(mat, eul1, eul2);
 
 	compatible_eul(eul1, oldrot);
 	compatible_eul(eul2, oldrot);
@@ -1318,6 +1359,19 @@ void mat3_to_compatible_eul(float eul[3], const float oldrot[3], float mat[3][3]
 		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);
+}
+
+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);
+}
 
 /************************** Arbitrary Order Eulers ***************************/
 
@@ -1403,10 +1457,10 @@ void eulO_to_quat(float q[4], const float e[3], const short order)
 /* Convert quaternion to Euler angles (in radians). */
 void quat_to_eulO(float e[3], short const order, const float q[4])
 {
-	float M[3][3];
+	float unit_mat[3][3];
 
-	quat_to_mat3(M, q);
-	mat3_to_eulO(e, order, M);
+	quat_to_mat3(unit_mat, q);
+	mat3_normalized_to_eulO(e, order, unit_mat);
 }
 
 /* Construct 3x3 matrix from Euler angles (in radians). */
@@ -1451,15 +1505,13 @@ void eulO_to_mat3(float M[3][3], const float e[3], const short order)
 }
 
 /* returns two euler calculation methods, so we can pick the best */
-static void mat3_to_eulo2(float M[3][3], float eul1[3], float eul2[3], const short order)
+static void mat3_normalized_to_eulo2(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 mat[3][3];
 	float cy;
 
-	/* process the matrix first */
-	normalize_m3_m3(mat, M);
+	BLI_ASSERT_UNIT_M3(mat);
 
 	cy = hypotf(mat[i][i], mat[i][j]);
 
@@ -1487,22 +1539,22 @@ static void mat3_to_eulo2(float M[3][3], float eul1[3], float eul2[3], const sho
 }
 
 /* Construct 4x4 matrix from Euler angles (in radians). */
-void eulO_to_mat4(float M[4][4], const float e[3], const short order)
+void eulO_to_mat4(float mat[4][4], const float e[3], const short order)
 {
-	float m[3][3];
+	float unit_mat[3][3];
 
 	/* for now, we'll just do this the slow way (i.e. copying matrices) */
-	eulO_to_mat3(m, e, order);
-	copy_m4_m3(M, m);
+	eulO_to_mat3(unit_mat, e, order);
+	copy_m4_m3(mat, unit_mat);
 }
 
 /* Convert 3x3 matrix to Euler angles (in radians). */
-void mat3_to_eulO(float eul[3], const short order, float M[3][3])
+void mat3_normalized_to_eulO(float eul[3], const short order, float m[3][3])
 {
 	float eul1[3], eul2[3];
 	float d1, d2;
 
-	mat3_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]);
@@ -1515,25 +1567,39 @@ void mat3_to_eulO(float eul[3], const short order, float M[3][3])
 		copy_v3_v3(eul, eul1);
 	}
 }
+void mat3_to_eulO(float eul[3], const short order, float m[3][3])
+{
+	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_to_eulO(float e[3], const short order, float M[4][4])
+void mat4_normalized_to_eulO(float eul[3], const short order, float m[4][4])
 {
-	float m[3][3];
+	float mat3[3][3];
 
 	/* for now, we'll just do this the slow way (i.e. copying matrices) */
-	copy_m3_m4(m, M);
-	normalize_m3(m);
-	mat3_to_eulO(e, order, m);
+	copy_m3_m4(mat3, m);
+	mat3_normalized_to_eulO(eul, order, mat3);
+}
+
+void mat4_to_eulO(float eul[3], const short order, float m[4][4])
+{
+	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_to_compatible_eulO(float eul[3], float oldrot[3], const short order, float mat[3][3])
+void mat3_normalized_to_compatible_eulO(float eul[3], float oldrot[3], const short order, float mat[3][3])
 {
 	float eul1[3], eul2[3];
 	float d1, d2;
 
-	mat3_to_eulo2(mat, eul1, eul2, order);
+	mat3_normalized_to_eulo2(mat, eul1, eul2, order);
 
 	compatible_eul(eul1, oldrot);
 	compatible_eul(eul2, oldrot);
@@ -1549,16 +1615,40 @@ void mat3_to_compatible_eulO(float eul[3], float oldrot[3], const short order, f
 		copy_v3_v3(eul, eul1);
 	}
 }
+void mat3_to_compatible_eulO(float eul[3], float oldrot[3], const short order, float mat[3][3])
+{
+	float unit_mat[3][3];
+
+	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], float oldrot[3], const short order, float m[4][4])
+{
+	float mat3[3][3];
 
-void mat4_to_compatible_eulO(float eul[3], float oldrot[3], const short order, float M[4][4])
+	/* 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], float oldrot[3], const short order, float m[4][4])
 {
-	float m[3][3];
+	float mat3[3][3];
 
 	/* for now, we'll just do this the slow way (i.e. copying matrices) */
-	copy_m3_m4(m, M);
-	normalize_m3(m);
-	mat3_to_compatible_eulO(eul, oldrot, order, m);
+	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], float oldrot[3], const short order, const float quat[4])
+{
+	float unit_mat[3][3];
+
+	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 */
 /* NOTE: is this safe to do with different axis orders? */
 
diff --git a/source/blender/editors/space_view3d/view3d_edit.c b/source/blender/editors/space_view3d/view3d_edit.c
index 53cacc0d4b3..a31bc618a0e 100644
--- a/source/blender/editors/space_view3d/view3d_edit.c
+++ b/source/blender/editors/space_view3d/view3d_edit.c
@@ -5031,7 +5031,7 @@ void ED_view3d_from_m4(float mat[4][4], float ofs[3], float quat[4], float *dist
 
 	/* Quat */
 	if (quat) {
-		mat3_to_quat(quat, nmat);
+		mat3_normalized_to_quat(quat, nmat);
 		invert_qt_normalized(quat);
 	}
 
diff --git a/source/blender/editors/space_view3d/view3d_view.c b/source/blender/editors/space_view3d/view3d_view.c
index e3736d90232..5fc7b57739e 100644
--- a/source/blender/editors/space_view3d/view3d_view.c
+++ b/source/blender/editors/space_view3d/view3d_view.c
@@ -849,18 +849,10 @@ void view3d_winmatrix_set(ARegion *ar, const View3D *v3d, const rctf *rect)
 
 static void obmat_to_viewmat(RegionView3D *rv3d, Object *ob)
 {
-	float bmat[4][4];
-	float tmat[3][3];
-	
 	rv3d->view = RV3D_VIEW_USER; /* don't show the grid */
-	
-	copy_m4_m4(bmat, ob->obmat);
-	normalize_m4(bmat);
-	invert_m4_m4(rv3d->viewmat, bmat);
-	
-	/* view quat calculation, needed for add object */
-	copy_m3_m4(tmat, rv3d->viewmat);
-	mat3_to_quat(rv3d->viewquat, tmat);
+	mat4_to_quat(rv3d->viewquat, ob->obmat);
+	invert_qt_normalized(rv3d->viewquat);
+	quat_to_mat4(rv3d->viewmat, rv3d->viewquat);
 }
 
 static float view3d_quat_axis[6][4] = {
diff --git a/source/blender/python/mathutils/mathutils_Matrix.c b/source/blender/python/mathutils/mathutils_Matrix.c
index 41baa9b089f..7ad20d41bf1 100644
--- a/source/blender/python/mathutils/mathutils_Matrix.c
+++ b/source/blender/python/mathutils/mathutils_Matrix.c
@@ -1120,8 +1120,7 @@ static PyObject *Matrix_to_euler(MatrixObject *self, PyObject *args)
 	float eul[3], eul_compatf[3];
 	EulerObject *eul_compat = NULL;
 
-	float tmat[3][3];
-	float (*mat)[3];
+	float mat[3][3];
 
 	if (BaseMath_ReadCallback(self) == -1)
 		return NULL;
@@ -1138,11 +1137,10 @@ static PyObject *Matrix_to_euler(MatrixObject *self, PyObject *args)
 
 	/*must be 3-4 cols, 3-4 rows, square matrix */
 	if (self->num_row == 3 && self->num_col == 3) {
-		mat = (float (*)[3])self->matrix;
+		copy_m3_m3(mat, (float (*)[3])self->matrix);
 	}
 	else if (self->num_row == 4 && self->num_col == 4) {
-		copy_m3_m4(tmat, (float (*)[4])self->matrix);
-		mat = tmat;
+		copy_m3_m4(mat, (float (*)[4])self->matrix);
 	}
 	else {
 		PyErr_SetString(PyExc_ValueError,
@@ -1158,13 +1156,15 @@ static PyObject *Matrix_to_euler(MatrixObject *self, PyObject *args)
 			return NULL;
 	}
 
+	normalize_m3(mat);
+
 	if (eul_compat) {
-		if (order == 1) mat3_to_compatible_eul(eul, eul_compatf, mat);
-		else            mat3_to_compatible_eulO(eul, eul_compatf, order, mat);
+		if (order == 1) mat3_normalized_to_compatible_eul(eul, eul_compatf, mat);
+		else            mat3_normalized_to_compatible_eulO(eul, eul_compatf, order, mat);
 	}
 	else {
-		if (order == 1) mat3_to_eul(eul, mat);
-		else mat3_to_eulO(eul, order, mat);
+		if (order == 1) mat3_normalized_to_eul(eul, mat);
+		else            mat3_normalized_to_eulO(eul, order, mat);
 	}
 
 	return Euler_CreatePyObject(eul, order, NULL);
diff --git a/source/blender/python/mathutils/mathutils_Quaternion.c b/source/blender/python/mathutils/mathutils_Quaternion.c
index 1752be6e306..898a002cf09 100644
--- a/source/blender/python/mathutils/mathutils_Quaternion.c
+++ b/source/blender/python/mathutils/mathutils_Quaternion.c
@@ -115,8 +115,8 @@ static PyObject *Quaternion_to_euler(QuaternionObject *self, PyObject *args)
 
 		quat_to_mat3(mat, tquat);
 
-		if (order == EULER_ORDER_XYZ)  mat3_to_compatible_eul(eul, eul_compat->eul, mat);
-		else                           mat3_to_compatible_eulO(eul, eul_compat->eul, order, mat);
+		if (order == EULER_ORDER_XYZ)  mat3_normalized_to_compatible_eul(eul, eul_compat->eul, mat);
+		else                           mat3_normalized_to_compatible_eulO(eul, eul_compat->eul, order, mat);
 	}
 	else {
 		if (order == EULER_ORDER_XYZ)  quat_to_eul(eul, tquat);