BLI_math: add mat3_normalized_to_* functions

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

1 files changed, 145 insertions, 55 deletions

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? */