diff options
author | Campbell Barton <ideasman42@gmail.com> | 2015-10-23 23:02:51 +0300 |
---|---|---|
committer | Campbell Barton <ideasman42@gmail.com> | 2015-10-23 23:02:51 +0300 |
commit | fbca69c69afb370ddc5a0b52e10d9db61c025752 (patch) | |
tree | 8dde2bedbc44a98af31d06bf2fa80a1fb04432e7 /source/blender/blenlib/intern | |
parent | 3a98426ed6d37a204b1d55834e0590dcb7990b47 (diff) |
BLI_math: add mat3_normalized_to_* functions
Many uses of matrices for rotation keep them normalized,
so no need to normalize each time.
Diffstat (limited to 'source/blender/blenlib/intern')
-rw-r--r-- | source/blender/blenlib/intern/math_matrix.c | 12 | ||||
-rw-r--r-- | source/blender/blenlib/intern/math_rotation.c | 200 |
2 files changed, 151 insertions, 61 deletions
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? */ |