diff options
Diffstat (limited to 'source/blender/blenlib/intern/math_rotation.c')
| -rw-r--r-- | source/blender/blenlib/intern/math_rotation.c | 36 |
1 files changed, 24 insertions, 12 deletions
diff --git a/source/blender/blenlib/intern/math_rotation.c b/source/blender/blenlib/intern/math_rotation.c index 3d5d47bc2e0..5f039e89e89 100644 --- a/source/blender/blenlib/intern/math_rotation.c +++ b/source/blender/blenlib/intern/math_rotation.c @@ -1043,11 +1043,12 @@ void expmap_to_quat(float r[4], const float expmap[3]) float angle; /* Obtain axis/angle representation. */ - angle = normalize_v3_v3(axis, expmap); - angle = angle_wrap_rad(angle); - - /* Convert to quaternion. */ - axis_angle_to_quat(r, axis, angle); + if (LIKELY((angle = normalize_v3_v3(axis, expmap)) != 0.0f)) { + axis_angle_normalized_to_quat(r, axis, angle_wrap_rad(angle)); + } + else { + unit_qt(r); + } } /******************************** XYZ Eulers *********************************/ @@ -1322,12 +1323,21 @@ static const RotOrderInfo rotOrders[] = { * NOTE: since we start at 1 for the values, but arrays index from 0, * there is -1 factor involved in this process... */ -#define GET_ROTATIONORDER_INFO(order) (assert(order >= 0 && order <= 6), (order < 1) ? &rotOrders[0] : &rotOrders[(order) - 1]) +static const RotOrderInfo *get_rotation_order_info(const short order) +{ + assert(order >= 0 && order <= 6); + if (order < 1) + return &rotOrders[0]; + else if (order < 6) + return &rotOrders[order - 1]; + else + return &rotOrders[5]; +} /* Construct quaternion from Euler angles (in radians). */ void eulO_to_quat(float q[4], const float e[3], const short order) { - const RotOrderInfo *R = GET_ROTATIONORDER_INFO(order); + const RotOrderInfo *R = get_rotation_order_info(order); short i = R->axis[0], j = R->axis[1], k = R->axis[2]; double ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss; double a[3]; @@ -1372,7 +1382,7 @@ void quat_to_eulO(float e[3], short const order, const float q[4]) /* Construct 3x3 matrix from Euler angles (in radians). */ void eulO_to_mat3(float M[3][3], const float e[3], const short order) { - const RotOrderInfo *R = GET_ROTATIONORDER_INFO(order); + const RotOrderInfo *R = get_rotation_order_info(order); short i = R->axis[0], j = R->axis[1], k = R->axis[2]; double ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss; @@ -1413,7 +1423,7 @@ 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) { - const RotOrderInfo *R = GET_ROTATIONORDER_INFO(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; @@ -1549,7 +1559,7 @@ void rotate_eulO(float beul[3], const short order, char axis, float ang) /* the matrix is written to as 3 axis vectors */ void eulO_to_gimbal_axis(float gmat[3][3], const float eul[3], const short order) { - const RotOrderInfo *R = GET_ROTATIONORDER_INFO(order); + const RotOrderInfo *R = get_rotation_order_info(order); float mat[3][3]; float teul[3]; @@ -1606,7 +1616,7 @@ void eulO_to_gimbal_axis(float gmat[3][3], const float eul[3], const short order void mat4_to_dquat(DualQuat *dq, float basemat[4][4], float mat[4][4]) { - float *t, *q, dscale[3], scale[3], basequat[4]; + float *t, *q, dscale[3], scale[3], basequat[4], mat3[3][3]; float baseRS[4][4], baseinv[4][4], baseR[4][4], baseRinv[4][4]; float R[4][4], S[4][4]; @@ -1619,7 +1629,9 @@ void mat4_to_dquat(DualQuat *dq, float basemat[4][4], float mat[4][4]) dscale[1] = scale[1] - 1.0f; dscale[2] = scale[2] - 1.0f; - if ((determinant_m4(mat) < 0.0f) || len_v3(dscale) > 1e-4f) { + copy_m3_m4(mat3, mat); + + if (!is_orthonormal_m3(mat3) || (determinant_m4(mat) < 0.0f) || len_v3(dscale) > 1e-4f) { /* extract R and S */ float tmp[4][4]; |