diff options
Diffstat (limited to 'source/blender/blenlib/intern/math_rotation.c')
| -rw-r--r-- | source/blender/blenlib/intern/math_rotation.c | 1544 |
1 files changed, 797 insertions, 747 deletions
diff --git a/source/blender/blenlib/intern/math_rotation.c b/source/blender/blenlib/intern/math_rotation.c index 5ae226bf6d5..dbba672100e 100644 --- a/source/blender/blenlib/intern/math_rotation.c +++ b/source/blender/blenlib/intern/math_rotation.c @@ -17,7 +17,7 @@ * * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. * All rights reserved. - + * The Original Code is: some of this file. * * ***** END GPL LICENSE BLOCK ***** @@ -40,25 +40,24 @@ /* convenience, avoids setting Y axis everywhere */ void unit_axis_angle(float axis[3], float *angle) { - axis[0]= 0.0f; - axis[1]= 1.0f; - axis[2]= 0.0f; - *angle= 0.0f; + axis[0] = 0.0f; + axis[1] = 1.0f; + axis[2] = 0.0f; + *angle = 0.0f; } - void unit_qt(float q[4]) { - q[0]= 1.0f; - q[1]= q[2]= q[3]= 0.0f; + q[0] = 1.0f; + q[1] = q[2] = q[3] = 0.0f; } void copy_qt_qt(float q1[4], const float q2[4]) { - q1[0]= q2[0]; - q1[1]= q2[1]; - q1[2]= q2[2]; - q1[3]= q2[3]; + q1[0] = q2[0]; + q1[1] = q2[1]; + q1[2] = q2[2]; + q1[3] = q2[3]; } int is_zero_qt(float *q) @@ -66,17 +65,17 @@ int is_zero_qt(float *q) return (q[0] == 0 && q[1] == 0 && q[2] == 0 && q[3] == 0); } -void mul_qt_qtqt(float *q, const float *q1, const float *q2) +void mul_qt_qtqt(float q[4], const float q1[4], const float q2[4]) { - float t0,t1,t2; + float t0, t1, t2; - t0= q1[0]*q2[0]-q1[1]*q2[1]-q1[2]*q2[2]-q1[3]*q2[3]; - t1= q1[0]*q2[1]+q1[1]*q2[0]+q1[2]*q2[3]-q1[3]*q2[2]; - t2= q1[0]*q2[2]+q1[2]*q2[0]+q1[3]*q2[1]-q1[1]*q2[3]; - q[3]= q1[0]*q2[3]+q1[3]*q2[0]+q1[1]*q2[2]-q1[2]*q2[1]; - q[0]=t0; - q[1]=t1; - q[2]=t2; + t0 = q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]; + t1 = q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]; + t2 = q1[0] * q2[2] + q1[2] * q2[0] + q1[3] * q2[1] - q1[1] * q2[3]; + q[3] = q1[0] * q2[3] + q1[3] * q2[0] + q1[1] * q2[2] - q1[2] * q2[1]; + q[0] = t0; + q[1] = t1; + q[2] = t2; } /* Assumes a unit quaternion */ @@ -84,18 +83,18 @@ void mul_qt_v3(const float q[4], float v[3]) { float t0, t1, t2; - t0= -q[1]*v[0]-q[2]*v[1]-q[3]*v[2]; - t1= q[0]*v[0]+q[2]*v[2]-q[3]*v[1]; - t2= q[0]*v[1]+q[3]*v[0]-q[1]*v[2]; - v[2]= q[0]*v[2]+q[1]*v[1]-q[2]*v[0]; - v[0]=t1; - v[1]=t2; + t0 = -q[1] * v[0] - q[2] * v[1] - q[3] * v[2]; + t1 = q[0] * v[0] + q[2] * v[2] - q[3] * v[1]; + t2 = q[0] * v[1] + q[3] * v[0] - q[1] * v[2]; + v[2] = q[0] * v[2] + q[1] * v[1] - q[2] * v[0]; + v[0] = t1; + v[1] = t2; - t1= t0*-q[1]+v[0]*q[0]-v[1]*q[3]+v[2]*q[2]; - t2= t0*-q[2]+v[1]*q[0]-v[2]*q[1]+v[0]*q[3]; - v[2]= t0*-q[3]+v[2]*q[0]-v[0]*q[2]+v[1]*q[1]; - v[0]=t1; - v[1]=t2; + t1 = t0 * -q[1] + v[0] * q[0] - v[1] * q[3] + v[2] * q[2]; + t2 = t0 * -q[2] + v[1] * q[0] - v[2] * q[1] + v[0] * q[3]; + v[2] = t0 * -q[3] + v[2] * q[0] - v[0] * q[2] + v[1] * q[1]; + v[0] = t1; + v[1] = t2; } void conjugate_qt(float q[4]) @@ -107,10 +106,10 @@ void conjugate_qt(float q[4]) float dot_qtqt(const float q1[4], const float q2[4]) { - return q1[0]*q2[0] + q1[1]*q2[1] + q1[2]*q2[2] + q1[3]*q2[3]; + return q1[0] * q2[0] + q1[1] * q2[1] + q1[2] * q2[2] + q1[3] * q2[3]; } -void invert_qt(float *q) +void invert_qt(float q[4]) { float f = dot_qtqt(q, q); @@ -118,17 +117,17 @@ void invert_qt(float *q) return; conjugate_qt(q); - mul_qt_fl(q, 1.0f/f); + mul_qt_fl(q, 1.0f / f); } -void invert_qt_qt(float *q1, const float *q2) +void invert_qt_qt(float q1[4], const float q2[4]) { copy_qt_qt(q1, q2); invert_qt(q1); } /* simple mult */ -void mul_qt_fl(float *q, const float f) +void mul_qt_fl(float q[4], const float f) { q[0] *= f; q[1] *= f; @@ -140,65 +139,64 @@ void sub_qt_qtqt(float q[4], const float q1[4], const float q2[4]) { float nq2[4]; - nq2[0]= -q2[0]; - nq2[1]= q2[1]; - nq2[2]= q2[2]; - nq2[3]= q2[3]; + nq2[0] = -q2[0]; + nq2[1] = q2[1]; + nq2[2] = q2[2]; + nq2[3] = q2[3]; mul_qt_qtqt(q, q1, nq2); } /* angular mult factor */ -void mul_fac_qt_fl(float *q, const float fac) +void mul_fac_qt_fl(float q[4], const float fac) { - float angle= fac*saacos(q[0]); /* quat[0]= cos(0.5*angle), but now the 0.5 and 2.0 rule out */ - - float co= (float)cos(angle); - float si= (float)sin(angle); - q[0]= co; - normalize_v3(q+1); - mul_v3_fl(q+1, si); + float angle = fac * saacos(q[0]); /* quat[0] = cos(0.5 * angle), but now the 0.5 and 2.0 rule out */ + + float co = (float)cos(angle); + float si = (float)sin(angle); + q[0] = co; + normalize_v3(q + 1); + mul_v3_fl(q + 1, si); } /* skip error check, currently only needed by mat3_to_quat_is_ok */ static void quat_to_mat3_no_error(float m[][3], const float q[4]) { - double q0, q1, q2, q3, qda,qdb,qdc,qaa,qab,qac,qbb,qbc,qcc; + double q0, q1, q2, q3, qda, qdb, qdc, qaa, qab, qac, qbb, qbc, qcc; - q0= M_SQRT2 * (double)q[0]; - q1= M_SQRT2 * (double)q[1]; - q2= M_SQRT2 * (double)q[2]; - q3= M_SQRT2 * (double)q[3]; + q0 = M_SQRT2 * (double)q[0]; + q1 = M_SQRT2 * (double)q[1]; + q2 = M_SQRT2 * (double)q[2]; + q3 = M_SQRT2 * (double)q[3]; - qda= q0*q1; - qdb= q0*q2; - qdc= q0*q3; - qaa= q1*q1; - qab= q1*q2; - qac= q1*q3; - qbb= q2*q2; - qbc= q2*q3; - qcc= q3*q3; + qda = q0 * q1; + qdb = q0 * q2; + qdc = q0 * q3; + qaa = q1 * q1; + qab = q1 * q2; + qac = q1 * q3; + qbb = q2 * q2; + qbc = q2 * q3; + qcc = q3 * q3; - m[0][0]= (float)(1.0-qbb-qcc); - m[0][1]= (float)(qdc+qab); - m[0][2]= (float)(-qdb+qac); + m[0][0] = (float)(1.0 - qbb - qcc); + m[0][1] = (float)(qdc + qab); + m[0][2] = (float)(-qdb + qac); - m[1][0]= (float)(-qdc+qab); - m[1][1]= (float)(1.0-qaa-qcc); - m[1][2]= (float)(qda+qbc); + m[1][0] = (float)(-qdc + qab); + m[1][1] = (float)(1.0 - qaa - qcc); + m[1][2] = (float)(qda + qbc); - m[2][0]= (float)(qdb+qac); - m[2][1]= (float)(-qda+qbc); - m[2][2]= (float)(1.0-qaa-qbb); + m[2][0] = (float)(qdb + qac); + m[2][1] = (float)(-qda + qbc); + m[2][2] = (float)(1.0 - qaa - qbb); } - void quat_to_mat3(float m[][3], const float q[4]) { #ifdef DEBUG float f; - if (!((f=dot_qtqt(q, q))==0.0f || (fabsf(f-1.0f) < (float)QUAT_EPSILON))) { + if (!((f = dot_qtqt(q, q)) == 0.0f || (fabsf(f - 1.0f) < (float)QUAT_EPSILON))) { fprintf(stderr, "Warning! quat_to_mat3() called with non-normalized: size %.8f *** report a bug ***\n", f); } #endif @@ -208,106 +206,106 @@ void quat_to_mat3(float m[][3], const float q[4]) 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; + double q0, q1, q2, q3, qda, qdb, qdc, qaa, qab, qac, qbb, qbc, qcc; #ifdef DEBUG - if (!((q0=dot_qtqt(q, q))==0.0f || (fabsf(q0-1.0) < QUAT_EPSILON))) { + if (!((q0 = dot_qtqt(q, q)) == 0.0f || (fabsf(q0 - 1.0) < QUAT_EPSILON))) { fprintf(stderr, "Warning! quat_to_mat4() called with non-normalized: size %.8f *** report a bug ***\n", (float)q0); } #endif - q0= M_SQRT2 * (double)q[0]; - q1= M_SQRT2 * (double)q[1]; - q2= M_SQRT2 * (double)q[2]; - q3= M_SQRT2 * (double)q[3]; - - qda= q0*q1; - qdb= q0*q2; - qdc= q0*q3; - qaa= q1*q1; - qab= q1*q2; - qac= q1*q3; - qbb= q2*q2; - qbc= q2*q3; - qcc= q3*q3; - - m[0][0]= (float)(1.0-qbb-qcc); - m[0][1]= (float)(qdc+qab); - m[0][2]= (float)(-qdb+qac); - m[0][3]= 0.0f; - - m[1][0]= (float)(-qdc+qab); - m[1][1]= (float)(1.0-qaa-qcc); - m[1][2]= (float)(qda+qbc); - m[1][3]= 0.0f; - - m[2][0]= (float)(qdb+qac); - m[2][1]= (float)(-qda+qbc); - m[2][2]= (float)(1.0-qaa-qbb); - m[2][3]= 0.0f; - - m[3][0]= m[3][1]= m[3][2]= 0.0f; - m[3][3]= 1.0f; -} - -void mat3_to_quat(float *q, float wmat[][3]) + q0 = M_SQRT2 * (double)q[0]; + q1 = M_SQRT2 * (double)q[1]; + q2 = M_SQRT2 * (double)q[2]; + q3 = M_SQRT2 * (double)q[3]; + + qda = q0 * q1; + qdb = q0 * q2; + qdc = q0 * q3; + qaa = q1 * q1; + qab = q1 * q2; + qac = q1 * q3; + qbb = q2 * q2; + qbc = q2 * q3; + qcc = q3 * q3; + + m[0][0] = (float)(1.0 - qbb - qcc); + m[0][1] = (float)(qdc + qab); + m[0][2] = (float)(-qdb + qac); + m[0][3] = 0.0f; + + m[1][0] = (float)(-qdc + qab); + m[1][1] = (float)(1.0 - qaa - qcc); + m[1][2] = (float)(qda + qbc); + m[1][3] = 0.0f; + + m[2][0] = (float)(qdb + qac); + m[2][1] = (float)(-qda + qbc); + m[2][2] = (float)(1.0 - qaa - qbb); + m[2][3] = 0.0f; + + m[3][0] = m[3][1] = m[3][2] = 0.0f; + m[3][3] = 1.0f; +} + +void mat3_to_quat(float q[4], float wmat[][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 */ - - tr= 0.25* (double)(1.0f+mat[0][0]+mat[1][1]+mat[2][2]); - - if (tr>(double)FLT_EPSILON) { - s= sqrt(tr); - q[0]= (float)s; - s= 1.0/(4.0*s); - q[1]= (float)((mat[1][2]-mat[2][1])*s); - q[2]= (float)((mat[2][0]-mat[0][2])*s); - q[3]= (float)((mat[0][1]-mat[1][0])*s); + normalize_m3(mat); /* this is needed AND a 'normalize_qt' in the end */ + + tr = 0.25 * (double)(1.0f + mat[0][0] + mat[1][1] + mat[2][2]); + + if (tr > (double)FLT_EPSILON) { + s = sqrt(tr); + q[0] = (float)s; + s = 1.0 / (4.0 * s); + q[1] = (float)((mat[1][2] - mat[2][1]) * s); + q[2] = (float)((mat[2][0] - mat[0][2]) * s); + q[3] = (float)((mat[0][1] - mat[1][0]) * s); } else { if (mat[0][0] > mat[1][1] && mat[0][0] > mat[2][2]) { - s= 2.0f*sqrtf(1.0f + mat[0][0] - mat[1][1] - mat[2][2]); - q[1]= (float)(0.25*s); + s = 2.0f * sqrtf(1.0f + mat[0][0] - mat[1][1] - mat[2][2]); + q[1] = (float)(0.25 * s); - s= 1.0/s; - q[0]= (float)((double)(mat[2][1] - mat[1][2])*s); - q[2]= (float)((double)(mat[1][0] + mat[0][1])*s); - q[3]= (float)((double)(mat[2][0] + mat[0][2])*s); + s = 1.0 / s; + q[0] = (float)((double)(mat[2][1] - mat[1][2]) * s); + q[2] = (float)((double)(mat[1][0] + mat[0][1]) * s); + q[3] = (float)((double)(mat[2][0] + mat[0][2]) * s); } else if (mat[1][1] > mat[2][2]) { - s= 2.0f*sqrtf(1.0f + mat[1][1] - mat[0][0] - mat[2][2]); - q[2]= (float)(0.25*s); + s = 2.0f * sqrtf(1.0f + mat[1][1] - mat[0][0] - mat[2][2]); + q[2] = (float)(0.25 * s); - s= 1.0/s; - q[0]= (float)((double)(mat[2][0] - mat[0][2])*s); - q[1]= (float)((double)(mat[1][0] + mat[0][1])*s); - q[3]= (float)((double)(mat[2][1] + mat[1][2])*s); + s = 1.0 / s; + q[0] = (float)((double)(mat[2][0] - mat[0][2]) * s); + q[1] = (float)((double)(mat[1][0] + mat[0][1]) * s); + q[3] = (float)((double)(mat[2][1] + mat[1][2]) * s); } else { - s= 2.0f*sqrtf(1.0f + mat[2][2] - mat[0][0] - mat[1][1]); - q[3]= (float)(0.25*s); + s = 2.0f * sqrtf(1.0f + mat[2][2] - mat[0][0] - mat[1][1]); + q[3] = (float)(0.25 * s); - s= 1.0/s; - q[0]= (float)((double)(mat[1][0] - mat[0][1])*s); - q[1]= (float)((double)(mat[2][0] + mat[0][2])*s); - q[2]= (float)((double)(mat[2][1] + mat[1][2])*s); + s = 1.0 / s; + q[0] = (float)((double)(mat[1][0] - mat[0][1]) * s); + q[1] = (float)((double)(mat[2][0] + mat[0][2]) * s); + q[2] = (float)((double)(mat[2][1] + mat[1][2]) * s); } } normalize_qt(q); } -void mat4_to_quat(float *q, float m[][4]) +void mat4_to_quat(float q[4], float m[][4]) { float mat[3][3]; - + copy_m3_m4(mat, m); - mat3_to_quat(q,mat); + mat3_to_quat(q, mat); } void mat3_to_quat_is_ok(float q[4], float wmat[3][3]) @@ -317,54 +315,53 @@ void mat3_to_quat_is_ok(float q[4], float wmat[3][3]) /* work on a copy */ copy_m3_m3(mat, wmat); normalize_m3(mat); - + /* rotate z-axis of matrix to z-axis */ - nor[0] = mat[2][1]; /* cross product with (0,0,1) */ - nor[1] = -mat[2][0]; + nor[0] = mat[2][1]; /* cross product with (0,0,1) */ + nor[1] = -mat[2][0]; nor[2] = 0.0; normalize_v3(nor); - - co= mat[2][2]; - angle= 0.5f*saacos(co); - - co= (float)cos(angle); - si= (float)sin(angle); - q1[0]= co; - q1[1]= -nor[0]*si; /* negative here, but why? */ - q1[2]= -nor[1]*si; - q1[3]= -nor[2]*si; + + co = mat[2][2]; + angle = 0.5f * saacos(co); + + co = (float)cos(angle); + si = (float)sin(angle); + q1[0] = co; + q1[1] = -nor[0] * si; /* negative here, but why? */ + q1[2] = -nor[1] * si; + q1[3] = -nor[2] * si; /* rotate back x-axis from mat, using inverse q1 */ - quat_to_mat3_no_error( matr,q1); + quat_to_mat3_no_error(matr, q1); invert_m3_m3(matn, matr); mul_m3_v3(matn, mat[0]); - + /* and align x-axes */ - angle= (float)(0.5*atan2(mat[0][1], mat[0][0])); - - co= (float)cos(angle); - si= (float)sin(angle); - q2[0]= co; - q2[1]= 0.0f; - q2[2]= 0.0f; - q2[3]= si; - + angle = (float)(0.5 * atan2(mat[0][1], mat[0][0])); + + co = (float)cos(angle); + si = (float)sin(angle); + q2[0] = co; + q2[1] = 0.0f; + q2[2] = 0.0f; + q2[3] = si; + mul_qt_qtqt(q, q1, q2); } - -float normalize_qt(float *q) +float normalize_qt(float q[4]) { float len; - - len= (float)sqrt(dot_qtqt(q, q)); - if (len!=0.0f) { - mul_qt_fl(q, 1.0f/len); + + len = (float)sqrt(dot_qtqt(q, q)); + if (len != 0.0f) { + mul_qt_fl(q, 1.0f / len); } else { - q[1]= 1.0f; - q[0]= q[2]= q[3]= 0.0f; + q[1] = 1.0f; + q[0] = q[2] = q[3] = 0.0f; } return len; @@ -377,19 +374,19 @@ float normalize_qt_qt(float r[4], const float q[4]) } /* note: expects vectors to be normalized */ -void rotation_between_vecs_to_quat(float *q, const float v1[3], const float v2[3]) +void rotation_between_vecs_to_quat(float q[4], const float v1[3], const float v2[3]) { float axis[3]; float angle; - + cross_v3_v3v3(axis, v1, v2); - + angle = angle_normalized_v3v3(v1, v2); - + axis_angle_to_quat(q, axis, angle); } -void rotation_between_quats_to_quat(float *q, const float q1[4], const float q2[4]) +void rotation_between_quats_to_quat(float q[4], const float q1[4], const float q2[4]) { float tquat[4]; double dot = 0.0f; @@ -405,141 +402,145 @@ void rotation_between_quats_to_quat(float *q, const float q1[4], const float q2[ mul_qt_qtqt(q, tquat, q2); } - void vec_to_quat(float q[4], const float vec[3], short axis, const short upflag) { float q2[4], nor[3], *fp, mat[3][3], angle, si, co, x2, y2, z2, len1; - + assert(axis >= 0 && axis <= 5); assert(upflag >= 0 && upflag <= 2); - + /* first rotate to axis */ - if (axis>2) { - x2= vec[0] ; y2= vec[1] ; z2= vec[2]; - axis-= 3; + if (axis > 2) { + x2 = vec[0]; + y2 = vec[1]; + z2 = vec[2]; + axis -= 3; } else { - x2= -vec[0] ; y2= -vec[1] ; z2= -vec[2]; + x2 = -vec[0]; + y2 = -vec[1]; + z2 = -vec[2]; } - - q[0]=1.0; - q[1]=q[2]=q[3]= 0.0; - len1= (float)sqrt(x2*x2+y2*y2+z2*z2); + q[0] = 1.0; + q[1] = q[2] = q[3] = 0.0; + + len1 = (float)sqrt(x2 * x2 + y2 * y2 + z2 * z2); if (len1 == 0.0f) return; /* nasty! I need a good routine for this... * problem is a rotation of an Y axis to the negative Y-axis for example. */ - if (axis==0) { /* x-axis */ - nor[0]= 0.0; - nor[1]= -z2; - nor[2]= y2; + if (axis == 0) { /* x-axis */ + nor[0] = 0.0; + nor[1] = -z2; + nor[2] = y2; - if (fabs(y2)+fabs(z2)<0.0001) - nor[1]= 1.0; + if (fabs(y2) + fabs(z2) < 0.0001) + nor[1] = 1.0; - co= x2; + co = x2; } - else if (axis==1) { /* y-axis */ - nor[0]= z2; - nor[1]= 0.0; - nor[2]= -x2; - - if (fabs(x2)+fabs(z2)<0.0001) - nor[2]= 1.0; - - co= y2; + else if (axis == 1) { /* y-axis */ + nor[0] = z2; + nor[1] = 0.0; + nor[2] = -x2; + + if (fabs(x2) + fabs(z2) < 0.0001) + nor[2] = 1.0; + + co = y2; } - else { /* z-axis */ - nor[0]= -y2; - nor[1]= x2; - nor[2]= 0.0; + else { /* z-axis */ + nor[0] = -y2; + nor[1] = x2; + nor[2] = 0.0; - if (fabs(x2)+fabs(y2)<0.0001) - nor[0]= 1.0; + if (fabs(x2) + fabs(y2) < 0.0001) + nor[0] = 1.0; - co= z2; + co = z2; } - co/= len1; + co /= len1; normalize_v3(nor); - - angle= 0.5f*saacos(co); - si= (float)sin(angle); - q[0]= (float)cos(angle); - q[1]= nor[0]*si; - q[2]= nor[1]*si; - q[3]= nor[2]*si; - - if (axis!=upflag) { - quat_to_mat3(mat,q); - - fp= mat[2]; - if (axis==0) { - if (upflag==1) angle= (float)(0.5*atan2(fp[2], fp[1])); - else angle= (float)(-0.5*atan2(fp[1], fp[2])); + + angle = 0.5f * saacos(co); + si = (float)sin(angle); + q[0] = (float)cos(angle); + q[1] = nor[0] * si; + q[2] = nor[1] * si; + q[3] = nor[2] * si; + + if (axis != upflag) { + quat_to_mat3(mat, q); + + fp = mat[2]; + if (axis == 0) { + if (upflag == 1) angle = (float)(0.5 * atan2(fp[2], fp[1])); + else angle = (float)(-0.5 * atan2(fp[1], fp[2])); } - else if (axis==1) { - if (upflag==0) angle= (float)(-0.5*atan2(fp[2], fp[0])); - else angle= (float)(0.5*atan2(fp[0], fp[2])); + else if (axis == 1) { + if (upflag == 0) angle = (float)(-0.5 * atan2(fp[2], fp[0])); + else angle = (float)(0.5 * atan2(fp[0], fp[2])); } else { - if (upflag==0) angle= (float)(0.5*atan2(-fp[1], -fp[0])); - else angle= (float)(-0.5*atan2(-fp[0], -fp[1])); + if (upflag == 0) angle = (float)(0.5 * atan2(-fp[1], -fp[0])); + else angle = (float)(-0.5 * atan2(-fp[0], -fp[1])); } - - co= cosf(angle); - si= sinf(angle)/len1; - q2[0]= co; - q2[1]= x2*si; - q2[2]= y2*si; - q2[3]= z2*si; - - mul_qt_qtqt(q,q2,q); + + co = cosf(angle); + si = sinf(angle) / len1; + q2[0] = co; + q2[1] = x2 * si; + q2[2] = y2 * si; + q2[3] = z2 * si; + + mul_qt_qtqt(q, q2, q); } } #if 0 + /* A & M Watt, Advanced animation and rendering techniques, 1992 ACM press */ -void QuatInterpolW(float *result, float *quat1, float *quat2, float t) +void QuatInterpolW(float *result, float quat1[4], float quat2[4], float t) { float omega, cosom, sinom, sc1, sc2; cosom = quat1[0] * quat2[0] + quat1[1] * quat2[1] + quat1[2] * quat2[2] + quat1[3] * quat2[3]; - + /* rotate around shortest angle */ if ((1.0f + cosom) > 0.0001f) { - + if ((1.0f - cosom) > 0.0001f) { omega = (float)acos(cosom); sinom = (float)sin(omega); sc1 = (float)sin((1.0 - t) * omega) / sinom; sc2 = (float)sin(t * omega) / sinom; - } + } else { sc1 = 1.0f - t; sc2 = t; } - result[0] = sc1*quat1[0] + sc2*quat2[0]; - result[1] = sc1*quat1[1] + sc2*quat2[1]; - result[2] = sc1*quat1[2] + sc2*quat2[2]; - result[3] = sc1*quat1[3] + sc2*quat2[3]; - } + result[0] = sc1 * quat1[0] + sc2 * quat2[0]; + result[1] = sc1 * quat1[1] + sc2 * quat2[1]; + result[2] = sc1 * quat1[2] + sc2 * quat2[2]; + result[3] = sc1 * quat1[3] + sc2 * quat2[3]; + } else { result[0] = quat2[3]; result[1] = -quat2[2]; result[2] = quat2[1]; result[3] = -quat2[0]; - - sc1 = (float)sin((1.0 - t)*M_PI_2); - sc2 = (float)sin(t*M_PI_2); - - result[0] = sc1*quat1[0] + sc2*result[0]; - result[1] = sc1*quat1[1] + sc2*result[1]; - result[2] = sc1*quat1[2] + sc2*result[2]; - result[3] = sc1*quat1[3] + sc2*result[3]; + + sc1 = (float)sin((1.0 - t) * M_PI_2); + sc2 = (float)sin(t * M_PI_2); + + result[0] = sc1 * quat1[0] + sc2 * result[0]; + result[1] = sc1 * quat1[1] + sc2 * result[1]; + result[2] = sc1 * quat1[2] + sc2 * result[2]; + result[3] = sc1 * quat1[3] + sc2 * result[3]; } } #endif @@ -553,18 +554,18 @@ void interp_qt_qtqt(float result[4], const float quat1[4], const float quat2[4], /* rotate around shortest angle */ if (cosom < 0.0f) { cosom = -cosom; - quat[0]= -quat1[0]; - quat[1]= -quat1[1]; - quat[2]= -quat1[2]; - quat[3]= -quat1[3]; - } + quat[0] = -quat1[0]; + quat[1] = -quat1[1]; + quat[2] = -quat1[2]; + quat[3] = -quat1[3]; + } else { - quat[0]= quat1[0]; - quat[1]= quat1[1]; - quat[2]= quat1[2]; - quat[3]= quat1[3]; + quat[0] = quat1[0]; + quat[1] = quat1[1]; + quat[2] = quat1[2]; + quat[3] = quat1[3]; } - + if ((1.0f - cosom) > 0.0001f) { omega = (float)acos(cosom); sinom = (float)sin(omega); @@ -572,10 +573,10 @@ void interp_qt_qtqt(float result[4], const float quat1[4], const float quat2[4], sc2 = (float)sin(t * omega) / sinom; } else { - sc1= 1.0f - t; - sc2= t; + sc1 = 1.0f - t; + sc2 = t; } - + result[0] = sc1 * quat[0] + sc2 * quat2[0]; result[1] = sc1 * quat[1] + sc2 * quat2[1]; result[2] = sc1 * quat[2] + sc2 * quat2[2]; @@ -584,57 +585,57 @@ void interp_qt_qtqt(float result[4], const float quat1[4], const float quat2[4], void add_qt_qtqt(float result[4], const float quat1[4], const float quat2[4], const float t) { - result[0]= quat1[0] + t*quat2[0]; - result[1]= quat1[1] + t*quat2[1]; - result[2]= quat1[2] + t*quat2[2]; - result[3]= quat1[3] + t*quat2[3]; + result[0] = quat1[0] + t * quat2[0]; + result[1] = quat1[1] + t * quat2[1]; + result[2] = quat1[2] + t * quat2[2]; + result[3] = quat1[3] + t * quat2[3]; } void tri_to_quat(float quat[4], const float v1[3], const float v2[3], const float v3[3]) { /* imaginary x-axis, y-axis triangle is being rotated */ float vec[3], q1[4], q2[4], n[3], si, co, angle, mat[3][3], imat[3][3]; - + /* move z-axis to face-normal */ - normal_tri_v3(vec,v1, v2, v3); + normal_tri_v3(vec, v1, v2, v3); - n[0]= vec[1]; - n[1]= -vec[0]; - n[2]= 0.0f; + n[0] = vec[1]; + n[1] = -vec[0]; + n[2] = 0.0f; normalize_v3(n); - - if (n[0]==0.0f && n[1]==0.0f) n[0]= 1.0f; - - angle= -0.5f*(float)saacos(vec[2]); - co= (float)cos(angle); - si= (float)sin(angle); - q1[0]= co; - q1[1]= n[0]*si; - q1[2]= n[1]*si; - q1[3]= 0.0f; - + + if (n[0] == 0.0f && n[1] == 0.0f) n[0] = 1.0f; + + angle = -0.5f * (float)saacos(vec[2]); + co = (float)cos(angle); + si = (float)sin(angle); + q1[0] = co; + q1[1] = n[0] * si; + q1[2] = n[1] * si; + q1[3] = 0.0f; + /* rotate back line v1-v2 */ - quat_to_mat3(mat,q1); + quat_to_mat3(mat, q1); invert_m3_m3(imat, mat); sub_v3_v3v3(vec, v2, v1); mul_m3_v3(imat, vec); /* what angle has this line with x-axis? */ - vec[2]= 0.0f; + vec[2] = 0.0f; normalize_v3(vec); - angle= (float)(0.5*atan2(vec[1], vec[0])); - co= (float)cos(angle); - si= (float)sin(angle); - q2[0]= co; - q2[1]= 0.0f; - q2[2]= 0.0f; - q2[3]= si; + angle = (float)(0.5 * atan2(vec[1], vec[0])); + co = (float)cos(angle); + si = (float)sin(angle); + q2[0] = co; + q2[1] = 0.0f; + q2[2] = 0.0f; + q2[3] = si; mul_qt_qtqt(quat, q1, q2); } -void print_qt(const char *str, const float q[4]) +void print_qt(const char *str, const float q[4]) { printf("%s: %.3f %.3f %.3f %.3f\n", str, q[0], q[1], q[2], q[3]); } @@ -657,7 +658,7 @@ void axis_angle_to_quat(float q[4], const float axis[3], float angle) q[0] = (float)cos(angle); q[1] = nor[0] * si; q[2] = nor[1] * si; - q[3] = nor[2] * si; + q[3] = nor[2] * si; } /* Quaternions to Axis Angle */ @@ -666,67 +667,67 @@ void quat_to_axis_angle(float axis[3], float *angle, const float q[4]) float ha, si; #ifdef DEBUG - if (!((ha=dot_qtqt(q, q))==0.0f || (fabsf(ha-1.0f) < (float)QUAT_EPSILON))) { + if (!((ha = dot_qtqt(q, q)) == 0.0f || (fabsf(ha - 1.0f) < (float)QUAT_EPSILON))) { fprintf(stderr, "Warning! quat_to_axis_angle() called with non-normalized: size %.8f *** report a bug ***\n", ha); } #endif /* calculate angle/2, and sin(angle/2) */ - ha= (float)acos(q[0]); - si= (float)sin(ha); - + ha = (float)acos(q[0]); + si = (float)sin(ha); + /* from half-angle to angle */ - *angle= ha * 2; - + *angle = ha * 2; + /* prevent division by zero for axis conversion */ if (fabs(si) < 0.0005) - si= 1.0f; - - axis[0]= q[1] / si; - axis[1]= q[2] / si; - axis[2]= q[3] / si; + si = 1.0f; + + axis[0] = q[1] / si; + axis[1] = q[2] / si; + axis[2] = q[3] / si; } /* Axis Angle to Euler Rotation */ void axis_angle_to_eulO(float eul[3], const short order, const float axis[3], const float angle) { float q[4]; - + /* use quaternions as intermediate representation for now... */ axis_angle_to_quat(q, axis, angle); - quat_to_eulO(eul, order,q); + quat_to_eulO(eul, order, q); } /* Euler Rotation to Axis Angle */ void eulO_to_axis_angle(float axis[3], float *angle, const float eul[3], const short order) { float q[4]; - + /* use quaternions as intermediate representation for now... */ - eulO_to_quat(q,eul, order); - quat_to_axis_angle(axis, angle,q); + eulO_to_quat(q, eul, order); + quat_to_axis_angle(axis, angle, q); } /* axis angle to 3x3 matrix - safer version (normalization of axis performed) */ void axis_angle_to_mat3(float mat[3][3], const float axis[3], const float angle) { float nor[3], nsi[3], co, si, ico; - + /* normalize the axis first (to remove unwanted scaling) */ if (normalize_v3_v3(nor, axis) == 0.0f) { unit_m3(mat); return; } - + /* now convert this to a 3x3 matrix */ - co= (float)cos(angle); - si= (float)sin(angle); - - ico= (1.0f - co); - nsi[0]= nor[0]*si; - nsi[1]= nor[1]*si; - nsi[2]= nor[2]*si; - + co = (float)cos(angle); + si = (float)sin(angle); + + ico = (1.0f - co); + nsi[0] = nor[0] * si; + nsi[1] = nor[1] * si; + nsi[2] = nor[2] * si; + mat[0][0] = ((nor[0] * nor[0]) * ico) + co; mat[0][1] = ((nor[0] * nor[1]) * ico) + nsi[2]; mat[0][2] = ((nor[0] * nor[2]) * ico) - nsi[1]; @@ -742,77 +743,75 @@ void axis_angle_to_mat3(float mat[3][3], const float axis[3], const float angle) void axis_angle_to_mat4(float mat[4][4], const float axis[3], const float angle) { float tmat[3][3]; - - axis_angle_to_mat3(tmat,axis, angle); + + axis_angle_to_mat3(tmat, axis, angle); unit_m4(mat); copy_m4_m3(mat, tmat); } /* 3x3 matrix to axis angle (see Mat4ToVecRot too) */ -void mat3_to_axis_angle(float axis[3], float *angle,float mat[3][3]) +void mat3_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_to_quat(q,mat); - quat_to_axis_angle(axis, angle,q); + mat3_to_quat(q, mat); + quat_to_axis_angle(axis, angle, q); } /* 4x4 matrix to axis angle (see Mat4ToVecRot too) */ -void mat4_to_axis_angle(float axis[3], float *angle,float mat[4][4]) +void mat4_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_to_quat(q,mat); - quat_to_axis_angle(axis, angle,q); + mat4_to_quat(q, mat); + quat_to_axis_angle(axis, angle, q); } - - void single_axis_angle_to_mat3(float mat[3][3], const char axis, const float angle) { - const float angle_cos= cosf(angle); - const float angle_sin= sinf(angle); - - switch(axis) { - case 'X': /* rotation around X */ - mat[0][0] = 1.0f; - mat[0][1] = 0.0f; - mat[0][2] = 0.0f; - mat[1][0] = 0.0f; - mat[1][1] = angle_cos; - mat[1][2] = angle_sin; - mat[2][0] = 0.0f; - mat[2][1] = -angle_sin; - mat[2][2] = angle_cos; - break; - case 'Y': /* rotation around Y */ - mat[0][0] = angle_cos; - mat[0][1] = 0.0f; - mat[0][2] = -angle_sin; - mat[1][0] = 0.0f; - mat[1][1] = 1.0f; - mat[1][2] = 0.0f; - mat[2][0] = angle_sin; - mat[2][1] = 0.0f; - mat[2][2] = angle_cos; - break; - case 'Z': /* rotation around Z */ - mat[0][0] = angle_cos; - mat[0][1] = angle_sin; - mat[0][2] = 0.0f; - mat[1][0] = -angle_sin; - mat[1][1] = angle_cos; - mat[1][2] = 0.0f; - mat[2][0] = 0.0f; - mat[2][1] = 0.0f; - mat[2][2] = 1.0f; - break; - default: - assert(0); + const float angle_cos = cosf(angle); + const float angle_sin = sinf(angle); + + switch (axis) { + case 'X': /* rotation around X */ + mat[0][0] = 1.0f; + mat[0][1] = 0.0f; + mat[0][2] = 0.0f; + mat[1][0] = 0.0f; + mat[1][1] = angle_cos; + mat[1][2] = angle_sin; + mat[2][0] = 0.0f; + mat[2][1] = -angle_sin; + mat[2][2] = angle_cos; + break; + case 'Y': /* rotation around Y */ + mat[0][0] = angle_cos; + mat[0][1] = 0.0f; + mat[0][2] = -angle_sin; + mat[1][0] = 0.0f; + mat[1][1] = 1.0f; + mat[1][2] = 0.0f; + mat[2][0] = angle_sin; + mat[2][1] = 0.0f; + mat[2][2] = angle_cos; + break; + case 'Z': /* rotation around Z */ + mat[0][0] = angle_cos; + mat[0][1] = angle_sin; + mat[0][2] = 0.0f; + mat[1][0] = -angle_sin; + mat[1][1] = angle_cos; + mat[1][2] = 0.0f; + mat[2][0] = 0.0f; + mat[2][1] = 0.0f; + mat[2][2] = 1.0f; + break; + default: + assert(0); } } @@ -824,33 +823,33 @@ void vec_rot_to_mat3(float mat[][3], const float vec[3], const float phi) { /* rotation of phi radials around vec */ float vx, vx2, vy, vy2, vz, vz2, co, si; - - vx= vec[0]; - vy= vec[1]; - vz= vec[2]; - vx2= vx*vx; - vy2= vy*vy; - vz2= vz*vz; - co= (float)cos(phi); - si= (float)sin(phi); - - mat[0][0]= vx2+co*(1.0f-vx2); - mat[0][1]= vx*vy*(1.0f-co)+vz*si; - mat[0][2]= vz*vx*(1.0f-co)-vy*si; - mat[1][0]= vx*vy*(1.0f-co)-vz*si; - mat[1][1]= vy2+co*(1.0f-vy2); - mat[1][2]= vy*vz*(1.0f-co)+vx*si; - mat[2][0]= vz*vx*(1.0f-co)+vy*si; - mat[2][1]= vy*vz*(1.0f-co)-vx*si; - mat[2][2]= vz2+co*(1.0f-vz2); + + vx = vec[0]; + vy = vec[1]; + vz = vec[2]; + vx2 = vx * vx; + vy2 = vy * vy; + vz2 = vz * vz; + co = (float)cos(phi); + si = (float)sin(phi); + + mat[0][0] = vx2 + co * (1.0f - vx2); + mat[0][1] = vx * vy * (1.0f - co) + vz * si; + mat[0][2] = vz * vx * (1.0f - co) - vy * si; + mat[1][0] = vx * vy * (1.0f - co) - vz * si; + mat[1][1] = vy2 + co * (1.0f - vy2); + mat[1][2] = vy * vz * (1.0f - co) + vx * si; + mat[2][0] = vz * vx * (1.0f - co) + vy * si; + mat[2][1] = vy * vz * (1.0f - co) - vx * si; + mat[2][2] = vz2 + co * (1.0f - vz2); } /* axis angle to 4x4 matrix */ void vec_rot_to_mat4(float mat[][4], const float vec[3], const float phi) { float tmat[3][3]; - - vec_rot_to_mat3(tmat,vec, phi); + + vec_rot_to_mat3(tmat, vec, phi); unit_m4(mat); copy_m4_m3(mat, tmat); } @@ -861,16 +860,16 @@ void vec_rot_to_quat(float *quat, const float vec[3], const float phi) /* rotation of phi radials around vec */ float si; - quat[1]= vec[0]; - quat[2]= vec[1]; - quat[3]= vec[2]; - - if (normalize_v3(quat+1) == 0.0f) { + quat[1] = vec[0]; + quat[2] = vec[1]; + quat[3] = vec[2]; + + if (normalize_v3(quat + 1) == 0.0f) { unit_qt(quat); } else { - quat[0]= (float)cos((double)phi/2.0); - si= (float)sin((double)phi/2.0); + quat[0] = (float)cos((double)phi / 2.0); + si = (float)sin((double)phi / 2.0); quat[1] *= si; quat[2] *= si; quat[3] *= si; @@ -883,27 +882,27 @@ void vec_rot_to_quat(float *quat, const float vec[3], const float phi) void eul_to_mat3(float mat[][3], const float eul[3]) { double ci, cj, ch, si, sj, sh, cc, cs, sc, ss; - - ci = cos(eul[0]); - cj = cos(eul[1]); + + ci = cos(eul[0]); + cj = cos(eul[1]); ch = cos(eul[2]); - si = sin(eul[0]); - sj = sin(eul[1]); + si = sin(eul[0]); + sj = sin(eul[1]); sh = sin(eul[2]); - cc = ci*ch; - cs = ci*sh; - sc = si*ch; - ss = si*sh; + cc = ci * ch; + cs = ci * sh; + sc = si * ch; + ss = si * sh; - mat[0][0] = (float)(cj*ch); - mat[1][0] = (float)(sj*sc-cs); - mat[2][0] = (float)(sj*cc+ss); - mat[0][1] = (float)(cj*sh); - mat[1][1] = (float)(sj*ss+cc); - mat[2][1] = (float)(sj*cs-sc); - mat[0][2] = (float)-sj; - mat[1][2] = (float)(cj*si); - mat[2][2] = (float)(cj*ci); + mat[0][0] = (float)(cj * ch); + mat[1][0] = (float)(sj * sc - cs); + mat[2][0] = (float)(sj * cc + ss); + mat[0][1] = (float)(cj * sh); + mat[1][1] = (float)(sj * ss + cc); + mat[2][1] = (float)(sj * cs - sc); + mat[0][2] = (float)-sj; + mat[1][2] = (float)(cj * si); + mat[2][2] = (float)(cj * ci); } @@ -911,75 +910,76 @@ void eul_to_mat3(float mat[][3], const float eul[3]) void eul_to_mat4(float mat[][4], const float eul[3]) { double ci, cj, ch, si, sj, sh, cc, cs, sc, ss; - - ci = cos(eul[0]); - cj = cos(eul[1]); + + ci = cos(eul[0]); + cj = cos(eul[1]); ch = cos(eul[2]); - si = sin(eul[0]); - sj = sin(eul[1]); + si = sin(eul[0]); + sj = sin(eul[1]); sh = sin(eul[2]); - cc = ci*ch; - cs = ci*sh; - sc = si*ch; - ss = si*sh; + cc = ci * ch; + cs = ci * sh; + sc = si * ch; + ss = si * sh; - mat[0][0] = (float)(cj*ch); - mat[1][0] = (float)(sj*sc-cs); - mat[2][0] = (float)(sj*cc+ss); - mat[0][1] = (float)(cj*sh); - mat[1][1] = (float)(sj*ss+cc); - mat[2][1] = (float)(sj*cs-sc); - mat[0][2] = (float)-sj; - mat[1][2] = (float)(cj*si); - mat[2][2] = (float)(cj*ci); + mat[0][0] = (float)(cj * ch); + mat[1][0] = (float)(sj * sc - cs); + mat[2][0] = (float)(sj * cc + ss); + mat[0][1] = (float)(cj * sh); + mat[1][1] = (float)(sj * ss + cc); + mat[2][1] = (float)(sj * cs - sc); + mat[0][2] = (float)-sj; + mat[1][2] = (float)(cj * si); + mat[2][2] = (float)(cj * ci); - mat[3][0]= mat[3][1]= mat[3][2]= mat[0][3]= mat[1][3]= mat[2][3]= 0.0f; - mat[3][3]= 1.0f; + mat[3][0] = mat[3][1] = mat[3][2] = mat[0][3] = mat[1][3] = mat[2][3] = 0.0f; + mat[3][3] = 1.0f; } /* returns two euler calculation methods, so we can pick the best */ + /* XYZ order */ static void mat3_to_eul2(float tmat[][3], float eul1[3], float eul2[3]) { float cy, quat[4], mat[3][3]; - - mat3_to_quat(quat,tmat); - quat_to_mat3(mat,quat); + + mat3_to_quat(quat, tmat); + quat_to_mat3(mat, quat); copy_m3_m3(mat, tmat); normalize_m3(mat); - - cy = (float)sqrt(mat[0][0]*mat[0][0] + mat[0][1]*mat[0][1]); - - if (cy > 16.0f*FLT_EPSILON) { - + + cy = (float)sqrt(mat[0][0] * mat[0][0] + mat[0][1] * mat[0][1]); + + if (cy > 16.0f * FLT_EPSILON) { + eul1[0] = (float)atan2(mat[1][2], mat[2][2]); eul1[1] = (float)atan2(-mat[0][2], cy); eul1[2] = (float)atan2(mat[0][1], mat[0][0]); - + eul2[0] = (float)atan2(-mat[1][2], -mat[2][2]); eul2[1] = (float)atan2(-mat[0][2], -cy); eul2[2] = (float)atan2(-mat[0][1], -mat[0][0]); - + } else { eul1[0] = (float)atan2(-mat[2][1], mat[1][1]); eul1[1] = (float)atan2(-mat[0][2], cy); eul1[2] = 0.0f; - + copy_v3_v3(eul2, eul1); } } /* XYZ order */ -void mat3_to_eul(float *eul,float tmat[][3]) +void mat3_to_eul(float *eul, float tmat[][3]) { float eul1[3], eul2[3]; - + mat3_to_eul2(tmat, eul1, eul2); - + /* return best, which is just the one with lowest values it in */ - if (fabs(eul1[0])+fabs(eul1[1])+fabs(eul1[2]) > fabs(eul2[0])+fabs(eul2[1])+fabs(eul2[2])) { + if (fabs(eul1[0]) + fabs(eul1[1]) + fabs(eul1[2]) > fabs(eul2[0]) + fabs(eul2[1]) + fabs(eul2[2])) { copy_v3_v3(eul, eul2); } else { @@ -988,13 +988,13 @@ void mat3_to_eul(float *eul,float tmat[][3]) } /* XYZ order */ -void mat4_to_eul(float *eul,float tmat[][4]) +void mat4_to_eul(float *eul, float tmat[][4]) { float tempMat[3][3]; copy_m3_m4(tempMat, tmat); normalize_m3(tempMat); - mat3_to_eul(eul,tempMat); + mat3_to_eul(eul, tempMat); } /* XYZ order */ @@ -1002,24 +1002,33 @@ void quat_to_eul(float *eul, const float quat[4]) { float mat[3][3]; - quat_to_mat3(mat,quat); - mat3_to_eul(eul,mat); + quat_to_mat3(mat, quat); + mat3_to_eul(eul, mat); } /* XYZ order */ void eul_to_quat(float *quat, const float eul[3]) { float ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss; - - ti = eul[0]*0.5f; tj = eul[1]*0.5f; th = eul[2]*0.5f; - ci = (float)cos(ti); cj = (float)cos(tj); ch = (float)cos(th); - si = (float)sin(ti); sj = (float)sin(tj); sh = (float)sin(th); - cc = ci*ch; cs = ci*sh; sc = si*ch; ss = si*sh; - - quat[0] = cj*cc + sj*ss; - quat[1] = cj*sc - sj*cs; - quat[2] = cj*ss + sj*cc; - quat[3] = cj*cs - sj*sc; + + ti = eul[0] * 0.5f; + tj = eul[1] * 0.5f; + th = eul[2] * 0.5f; + ci = cosf(ti); + cj = cosf(tj); + ch = cosf(th); + si = sinf(ti); + sj = sinf(tj); + sh = sinf(th); + cc = ci * ch; + cs = ci * sh; + sc = si * ch; + ss = si * sh; + + quat[0] = cj * cc + sj * ss; + quat[1] = cj * sc - sj * cs; + quat[2] = cj * ss + sj * cc; + quat[3] = cj * cs - sj * sc; } /* XYZ order */ @@ -1029,99 +1038,116 @@ void rotate_eul(float *beul, const char axis, const float ang) assert(axis >= 'X' && axis <= 'Z'); - eul[0]= eul[1]= eul[2]= 0.0f; - if (axis=='X') eul[0]= ang; - else if (axis=='Y') eul[1]= ang; - else eul[2]= ang; - - eul_to_mat3(mat1,eul); - eul_to_mat3(mat2,beul); - + eul[0] = eul[1] = eul[2] = 0.0f; + if (axis == 'X') eul[0] = ang; + else if (axis == 'Y') eul[1] = ang; + else eul[2] = ang; + + eul_to_mat3(mat1, eul); + eul_to_mat3(mat2, beul); + mul_m3_m3m3(totmat, mat2, mat1); - - mat3_to_eul(beul,totmat); - + + mat3_to_eul(beul, totmat); + } /* exported to transform.c */ + /* order independent! */ void compatible_eul(float eul[3], const float oldrot[3]) { float dx, dy, dz; - + /* correct differences of about 360 degrees first */ - dx= eul[0] - oldrot[0]; - dy= eul[1] - oldrot[1]; - dz= eul[2] - oldrot[2]; - + dx = eul[0] - oldrot[0]; + dy = eul[1] - oldrot[1]; + dz = eul[2] - oldrot[2]; + while (fabs(dx) > 5.1) { - if (dx > 0.0f) eul[0] -= 2.0f*(float)M_PI; else eul[0]+= 2.0f*(float)M_PI; - dx= eul[0] - oldrot[0]; + if (dx > 0.0f) eul[0] -= 2.0f * (float)M_PI; + else eul[0] += 2.0f * (float)M_PI; + dx = eul[0] - oldrot[0]; } while (fabs(dy) > 5.1) { - if (dy > 0.0f) eul[1] -= 2.0f*(float)M_PI; else eul[1]+= 2.0f*(float)M_PI; - dy= eul[1] - oldrot[1]; + if (dy > 0.0f) eul[1] -= 2.0f * (float)M_PI; + else eul[1] += 2.0f * (float)M_PI; + dy = eul[1] - oldrot[1]; } while (fabs(dz) > 5.1) { - if (dz > 0.0f) eul[2] -= 2.0f*(float)M_PI; else eul[2]+= 2.0f*(float)M_PI; - dz= eul[2] - oldrot[2]; + if (dz > 0.0f) eul[2] -= 2.0f * (float)M_PI; + else eul[2] += 2.0f * (float)M_PI; + dz = eul[2] - oldrot[2]; } - - /* is 1 of the axis rotations larger than 180 degrees and the other small? NO ELSE IF!! */ - if (fabs(dx) > 3.2 && fabs(dy)<1.6 && fabs(dz)<1.6) { - if (dx > 0.0f) eul[0] -= 2.0f*(float)M_PI; else eul[0]+= 2.0f*(float)M_PI; + + /* is 1 of the axis rotations larger than 180 degrees and the other small? NO ELSE IF!! */ + if (fabs(dx) > 3.2 && fabs(dy) < 1.6 && fabs(dz) < 1.6) { + if (dx > 0.0f) eul[0] -= 2.0f * (float)M_PI; + else eul[0] += 2.0f * (float)M_PI; } - if (fabs(dy) > 3.2 && fabs(dz)<1.6 && fabs(dx)<1.6) { - if (dy > 0.0f) eul[1] -= 2.0f*(float)M_PI; else eul[1]+= 2.0f*(float)M_PI; + if (fabs(dy) > 3.2 && fabs(dz) < 1.6 && fabs(dx) < 1.6) { + if (dy > 0.0f) eul[1] -= 2.0f * (float)M_PI; + else eul[1] += 2.0f * (float)M_PI; } - if (fabs(dz) > 3.2 && fabs(dx)<1.6 && fabs(dy)<1.6) { - if (dz > 0.0f) eul[2] -= 2.0f*(float)M_PI; else eul[2]+= 2.0f*(float)M_PI; + if (fabs(dz) > 3.2 && fabs(dx) < 1.6 && fabs(dy) < 1.6) { + if (dz > 0.0f) eul[2] -= 2.0f * (float)M_PI; + else eul[2] += 2.0f * (float)M_PI; } - + /* the method below was there from ancient days... but why! probably because the code sucks :) */ -#if 0 +#if 0 /* calc again */ - dx= eul[0] - oldrot[0]; - dy= eul[1] - oldrot[1]; - dz= eul[2] - oldrot[2]; - + dx = eul[0] - oldrot[0]; + dy = eul[1] - oldrot[1]; + dz = eul[2] - oldrot[2]; + /* special case, tested for x-z */ - + if ((fabs(dx) > 3.1 && fabs(dz) > 1.5) || (fabs(dx) > 1.5 && fabs(dz) > 3.1)) { - if (dx > 0.0) eul[0] -= M_PI; else eul[0]+= M_PI; - if (eul[1] > 0.0) eul[1]= M_PI - eul[1]; else eul[1]= -M_PI - eul[1]; - if (dz > 0.0) eul[2] -= M_PI; else eul[2]+= M_PI; - + if (dx > 0.0) eul[0] -= M_PI; + else eul[0] += M_PI; + if (eul[1] > 0.0) eul[1] = M_PI - eul[1]; + else eul[1] = -M_PI - eul[1]; + if (dz > 0.0) eul[2] -= M_PI; + else eul[2] += M_PI; + } else if ((fabs(dx) > 3.1 && fabs(dy) > 1.5) || (fabs(dx) > 1.5 && fabs(dy) > 3.1)) { - if (dx > 0.0) eul[0] -= M_PI; else eul[0]+= M_PI; - if (dy > 0.0) eul[1] -= M_PI; else eul[1]+= M_PI; - if (eul[2] > 0.0) eul[2]= M_PI - eul[2]; else eul[2]= -M_PI - eul[2]; + if (dx > 0.0) eul[0] -= M_PI; + else eul[0] += M_PI; + if (dy > 0.0) eul[1] -= M_PI; + else eul[1] += M_PI; + if (eul[2] > 0.0) eul[2] = M_PI - eul[2]; + else eul[2] = -M_PI - eul[2]; } else if ((fabs(dy) > 3.1 && fabs(dz) > 1.5) || (fabs(dy) > 1.5 && fabs(dz) > 3.1)) { - if (eul[0] > 0.0) eul[0]= M_PI - eul[0]; else eul[0]= -M_PI - eul[0]; - if (dy > 0.0) eul[1] -= M_PI; else eul[1]+= M_PI; - if (dz > 0.0) eul[2] -= M_PI; else eul[2]+= M_PI; + if (eul[0] > 0.0) eul[0] = M_PI - eul[0]; + else eul[0] = -M_PI - eul[0]; + if (dy > 0.0) eul[1] -= M_PI; + else eul[1] += M_PI; + if (dz > 0.0) eul[2] -= M_PI; + else eul[2] += M_PI; } -#endif +#endif } /* 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]) { float eul1[3], eul2[3]; float d1, d2; - + mat3_to_eul2(mat, eul1, eul2); - + compatible_eul(eul1, oldrot); compatible_eul(eul2, oldrot); - - d1= (float)fabs(eul1[0]-oldrot[0]) + (float)fabs(eul1[1]-oldrot[1]) + (float)fabs(eul1[2]-oldrot[2]); - d2= (float)fabs(eul2[0]-oldrot[0]) + (float)fabs(eul2[1]-oldrot[1]) + (float)fabs(eul2[2]-oldrot[2]); - + + d1 = (float)fabs(eul1[0] - oldrot[0]) + (float)fabs(eul1[1] - oldrot[1]) + (float)fabs(eul1[2] - oldrot[2]); + d2 = (float)fabs(eul2[0] - oldrot[0]) + (float)fabs(eul2[1] - oldrot[1]) + (float)fabs(eul2[2] - oldrot[2]); + /* return best, which is just the one with lowest difference */ if (d1 > d2) { copy_v3_v3(eul, eul2); @@ -1129,13 +1155,13 @@ void mat3_to_compatible_eul(float eul[3], const float oldrot[3], float mat[][3]) else { copy_v3_v3(eul, eul1); } - + } /************************** Arbitrary Order Eulers ***************************/ /* Euler Rotation Order Code: - * was adapted from + * was adapted from * ANSI C code from the article * "Euler Angle Conversion" * by Ken Shoemake, shoemake@graphics.cis.upenn.edu @@ -1146,13 +1172,13 @@ void mat3_to_compatible_eul(float eul[3], const float oldrot[3], float mat[][3]) /* Type for rotation order info - see wiki for derivation details */ typedef struct RotOrderInfo { short axis[3]; - short parity; /* parity of axis permutation (even=0, odd=1) - 'n' in original code */ + short parity; /* parity of axis permutation (even=0, odd=1) - 'n' in original code */ } RotOrderInfo; -/* Array of info for Rotation Order calculations +/* Array of info for Rotation Order calculations * WARNING: must be kept in same order as eEulerRotationOrders */ -static RotOrderInfo rotOrders[]= { +static RotOrderInfo rotOrders[] = { /* i, j, k, n */ {{0, 1, 2}, 0}, // XYZ {{0, 2, 1}, 1}, // XZY @@ -1162,114 +1188,136 @@ static RotOrderInfo rotOrders[]= { {{2, 1, 0}, 1} // ZYX }; -/* Get relevant pointer to rotation order set from the array - * NOTE: since we start at 1 for the values, but arrays index from 0, +/* Get relevant pointer to rotation order set from the array + * 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]) +#define GET_ROTATIONORDER_INFO(order) (assert(order >= 0 && order <= 6), (order < 1) ? &rotOrders[0] : &rotOrders[(order) - 1]) /* Construct quaternion from Euler angles (in radians). */ void eulO_to_quat(float q[4], const float e[3], const short order) { - RotOrderInfo *R= GET_ROTATIONORDER_INFO(order); - short i=R->axis[0], j=R->axis[1], k=R->axis[2]; + RotOrderInfo *R = GET_ROTATIONORDER_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]; - + ti = e[i] * 0.5f; tj = e[j] * (R->parity ? -0.5f : 0.5f); th = e[k] * 0.5f; - ci = cos(ti); cj = cos(tj); ch = cos(th); - si = sin(ti); sj = sin(tj); sh = sin(th); - - cc = ci*ch; cs = ci*sh; - sc = si*ch; ss = si*sh; - - a[i] = cj*sc - sj*cs; - a[j] = cj*ss + sj*cc; - a[k] = cj*cs - sj*sc; - - q[0] = cj*cc + sj*ss; + ci = cos(ti); + cj = cos(tj); + ch = cos(th); + si = sin(ti); + sj = sin(tj); + sh = sin(th); + + cc = ci * ch; + cs = ci * sh; + sc = si * ch; + ss = si * sh; + + a[i] = cj * sc - sj * cs; + a[j] = cj * ss + sj * cc; + a[k] = cj * cs - sj * sc; + + q[0] = cj * cc + sj * ss; q[1] = a[0]; q[2] = a[1]; q[3] = a[2]; - - if (R->parity) q[j+1] = -q[j+1]; + + if (R->parity) q[j + 1] = -q[j + 1]; } /* 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]; - - quat_to_mat3(M,q); - mat3_to_eulO(e, order,M); + + quat_to_mat3(M, q); + mat3_to_eulO(e, order, M); } /* Construct 3x3 matrix from Euler angles (in radians). */ void eulO_to_mat3(float M[3][3], const float e[3], const short order) { - RotOrderInfo *R= GET_ROTATIONORDER_INFO(order); - short i=R->axis[0], j=R->axis[1], k=R->axis[2]; + RotOrderInfo *R = GET_ROTATIONORDER_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; - + if (R->parity) { - ti = -e[i]; tj = -e[j]; th = -e[k]; + ti = -e[i]; + tj = -e[j]; + th = -e[k]; } else { - ti = e[i]; tj = e[j]; th = e[k]; + ti = e[i]; + tj = e[j]; + th = e[k]; } - - ci = cos(ti); cj = cos(tj); ch = cos(th); - si = sin(ti); sj = sin(tj); sh = sin(th); - - cc = ci*ch; cs = ci*sh; - sc = si*ch; ss = si*sh; - - M[i][i] = cj*ch; M[j][i] = sj*sc-cs; M[k][i] = sj*cc+ss; - M[i][j] = cj*sh; M[j][j] = sj*ss+cc; M[k][j] = sj*cs-sc; - M[i][k] = -sj; M[j][k] = cj*si; M[k][k] = cj*ci; + + ci = cos(ti); + cj = cos(tj); + ch = cos(th); + si = sin(ti); + sj = sin(tj); + sh = sin(th); + + cc = ci * ch; + cs = ci * sh; + sc = si * ch; + ss = si * sh; + + M[i][i] = cj * ch; + M[j][i] = sj * sc - cs; + M[k][i] = sj * cc + ss; + M[i][j] = cj * sh; + M[j][j] = sj * ss + cc; + M[k][j] = sj * cs - sc; + M[i][k] = -sj; + M[j][k] = cj * si; + M[k][k] = cj * ci; } /* returns two euler calculation methods, so we can pick the best */ static void mat3_to_eulo2(float M[3][3], float *e1, float *e2, short order) { - RotOrderInfo *R= GET_ROTATIONORDER_INFO(order); - short i=R->axis[0], j=R->axis[1], k=R->axis[2]; + RotOrderInfo *R = GET_ROTATIONORDER_INFO(order); + short i = R->axis[0], j = R->axis[1], k = R->axis[2]; float m[3][3]; double cy; - + /* process the matrix first */ copy_m3_m3(m, M); normalize_m3(m); - - cy= sqrt(m[i][i]*m[i][i] + m[i][j]*m[i][j]); - - if (cy > 16.0*(double)FLT_EPSILON) { + + cy = sqrt(m[i][i] * m[i][i] + m[i][j] * m[i][j]); + + if (cy > 16.0 * (double)FLT_EPSILON) { e1[i] = atan2(m[j][k], m[k][k]); e1[j] = atan2(-m[i][k], cy); e1[k] = atan2(m[i][j], m[i][i]); - + e2[i] = atan2(-m[j][k], -m[k][k]); e2[j] = atan2(-m[i][k], -cy); e2[k] = atan2(-m[i][j], -m[i][i]); - } + } else { e1[i] = atan2(-m[k][j], m[j][j]); e1[j] = atan2(-m[i][k], cy); e1[k] = 0; - + copy_v3_v3(e2, e1); } - + if (R->parity) { - e1[0] = -e1[0]; - e1[1] = -e1[1]; + e1[0] = -e1[0]; + e1[1] = -e1[1]; e1[2] = -e1[2]; - - e2[0] = -e2[0]; - e2[1] = -e2[1]; + + e2[0] = -e2[0]; + e2[1] = -e2[1]; e2[2] = -e2[2]; } } @@ -1278,23 +1326,22 @@ static void mat3_to_eulo2(float M[3][3], float *e1, float *e2, short order) void eulO_to_mat4(float M[4][4], const float e[3], const short order) { float m[3][3]; - + /* for now, we'll just do this the slow way (i.e. copying matrices) */ normalize_m3(m); - eulO_to_mat3(m,e, order); + eulO_to_mat3(m, e, order); copy_m4_m3(M, m); } - /* Convert 3x3 matrix to Euler angles (in radians). */ -void mat3_to_eulO(float eul[3], const short order,float M[3][3]) +void mat3_to_eulO(float eul[3], const short order, float M[3][3]) { float eul1[3], eul2[3]; - + mat3_to_eulo2(M, eul1, eul2, order); - + /* return best, which is just the one with lowest values it in */ - if (fabs(eul1[0])+fabs(eul1[1])+fabs(eul1[2]) > fabs(eul2[0])+fabs(eul2[1])+fabs(eul2[2])) { + if (fabs(eul1[0]) + fabs(eul1[1]) + fabs(eul1[2]) > fabs(eul2[0]) + fabs(eul2[1]) + fabs(eul2[2])) { copy_v3_v3(eul, eul2); } else { @@ -1303,30 +1350,30 @@ void mat3_to_eulO(float eul[3], const short order,float M[3][3]) } /* Convert 4x4 matrix to Euler angles (in radians). */ -void mat4_to_eulO(float e[3], const short order,float M[4][4]) +void mat4_to_eulO(float e[3], const short order, float M[4][4]) { float m[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); + mat3_to_eulO(e, order, m); } /* uses 2 methods to retrieve eulers, and picks the closest */ -void mat3_to_compatible_eulO(float eul[3], float oldrot[3], short order,float mat[3][3]) +void mat3_to_compatible_eulO(float eul[3], float oldrot[3], short order, float mat[3][3]) { float eul1[3], eul2[3]; float d1, d2; - + mat3_to_eulo2(mat, eul1, eul2, order); - + compatible_eul(eul1, oldrot); compatible_eul(eul2, oldrot); - - d1= fabsf(eul1[0]-oldrot[0]) + fabsf(eul1[1]-oldrot[1]) + fabsf(eul1[2]-oldrot[2]); - d2= fabsf(eul2[0]-oldrot[0]) + fabsf(eul2[1]-oldrot[1]) + fabsf(eul2[2]-oldrot[2]); - + + d1 = fabsf(eul1[0] - oldrot[0]) + fabsf(eul1[1] - oldrot[1]) + fabsf(eul1[2] - oldrot[2]); + d2 = fabsf(eul2[0] - oldrot[0]) + fabsf(eul2[1] - oldrot[1]) + fabsf(eul2[2] - oldrot[2]); + /* return best, which is just the one with lowest difference */ if (d1 > d2) copy_v3_v3(eul, eul2); @@ -1334,10 +1381,10 @@ void mat3_to_compatible_eulO(float eul[3], float oldrot[3], short order,float ma copy_v3_v3(eul, eul1); } -void mat4_to_compatible_eulO(float eul[3], float oldrot[3], short order,float M[4][4]) +void mat4_to_compatible_eulO(float eul[3], float oldrot[3], short order, float M[4][4]) { float m[3][3]; - + /* for now, we'll just do this the slow way (i.e. copying matrices) */ copy_m3_m4(m, M); normalize_m3(m); @@ -1345,47 +1392,48 @@ void mat4_to_compatible_eulO(float eul[3], float oldrot[3], short order,float M[ } /* rotate the given euler by the given angle on the specified axis */ // NOTE: is this safe to do with different axis orders? + void rotate_eulO(float beul[3], short order, char axis, float ang) { float eul[3], mat1[3][3], mat2[3][3], totmat[3][3]; assert(axis >= 'X' && axis <= 'Z'); - eul[0]= eul[1]= eul[2]= 0.0f; - if (axis=='X') - eul[0]= ang; - else if (axis=='Y') - eul[1]= ang; - else - eul[2]= ang; - - eulO_to_mat3(mat1,eul, order); - eulO_to_mat3(mat2,beul, order); - + eul[0] = eul[1] = eul[2] = 0.0f; + if (axis == 'X') + eul[0] = ang; + else if (axis == 'Y') + eul[1] = ang; + else + eul[2] = ang; + + eulO_to_mat3(mat1, eul, order); + eulO_to_mat3(mat2, beul, order); + mul_m3_m3m3(totmat, mat2, mat1); - - mat3_to_eulO(beul, order,totmat); + + mat3_to_eulO(beul, order, totmat); } /* the matrix is written to as 3 axis vectors */ void eulO_to_gimbal_axis(float gmat[][3], const float eul[3], const short order) { - RotOrderInfo *R= GET_ROTATIONORDER_INFO(order); + RotOrderInfo *R = GET_ROTATIONORDER_INFO(order); float mat[3][3]; float teul[3]; /* first axis is local */ - eulO_to_mat3(mat,eul, order); + eulO_to_mat3(mat, eul, order); copy_v3_v3(gmat[R->axis[0]], mat[R->axis[0]]); - + /* second axis is local minus first rotation */ copy_v3_v3(teul, eul); teul[R->axis[0]] = 0; - eulO_to_mat3(mat,teul, order); + eulO_to_mat3(mat, teul, order); copy_v3_v3(gmat[R->axis[1]], mat[R->axis[1]]); - - + + /* Last axis is global */ gmat[R->axis[2]][0] = 0; gmat[R->axis[2]][1] = 0; @@ -1401,7 +1449,7 @@ void eulO_to_gimbal_axis(float gmat[][3], const float eul[3], const short order) * * Version 1.0.0, February 7th, 2007 * - * Copyright (C) 2006-2007 University of Dublin, Trinity College, All Rights + * Copyright (C) 2006-2007 University of Dublin, Trinity College, All Rights * Reserved * * This software is provided 'as-is', without any express or implied @@ -1427,7 +1475,7 @@ void eulO_to_gimbal_axis(float gmat[][3], const float eul[3], const short order) * - added support for scaling */ -void mat4_to_dquat(DualQuat *dq,float basemat[][4], float mat[][4]) +void mat4_to_dquat(DualQuat *dq, float basemat[][4], float mat[][4]) { float *t, *q, dscale[3], scale[3], basequat[4]; float baseRS[4][4], baseinv[4][4], baseR[4][4], baseRinv[4][4]; @@ -1436,16 +1484,18 @@ void mat4_to_dquat(DualQuat *dq,float basemat[][4], float mat[][4]) /* split scaling and rotation, there is probably a faster way to do * this, it's done like this now to correctly get negative scaling */ mult_m4_m4m4(baseRS, mat, basemat); - mat4_to_size(scale,baseRS); + mat4_to_size(scale, baseRS); copy_v3_v3(dscale, scale); - dscale[0] -= 1.0f; dscale[1] -= 1.0f; dscale[2] -= 1.0f; + dscale[0] -= 1.0f; + dscale[1] -= 1.0f; + dscale[2] -= 1.0f; if ((determinant_m4(mat) < 0.0f) || len_v3(dscale) > 1e-4f) { /* extract R and S */ float tmp[4][4]; - /* extra orthogonalize, to avoid flipping with stretched bones */ + /* extra orthogonalize, to avoid flipping with stretched bones */ copy_m4_m4(tmp, baseRS); orthogonalize_m4(tmp, 1); mat4_to_quat(basequat, tmp); @@ -1461,78 +1511,78 @@ void mat4_to_dquat(DualQuat *dq,float basemat[][4], float mat[][4]) /* set scaling part */ mul_serie_m4(dq->scale, basemat, S, baseinv, NULL, NULL, NULL, NULL, NULL); - dq->scale_weight= 1.0f; + dq->scale_weight = 1.0f; } else { /* matrix does not contain scaling */ copy_m4_m4(R, mat); - dq->scale_weight= 0.0f; + dq->scale_weight = 0.0f; } /* non-dual part */ - mat4_to_quat(dq->quat,R); + mat4_to_quat(dq->quat, R); /* dual part */ - t= R[3]; - q= dq->quat; - dq->trans[0]= -0.5f*(t[0]*q[1] + t[1]*q[2] + t[2]*q[3]); - dq->trans[1]= 0.5f*(t[0]*q[0] + t[1]*q[3] - t[2]*q[2]); - dq->trans[2]= 0.5f*(-t[0]*q[3] + t[1]*q[0] + t[2]*q[1]); - dq->trans[3]= 0.5f*(t[0]*q[2] - t[1]*q[1] + t[2]*q[0]); + t = R[3]; + q = dq->quat; + dq->trans[0] = -0.5f * (t[0] * q[1] + t[1] * q[2] + t[2] * q[3]); + dq->trans[1] = 0.5f * (t[0] * q[0] + t[1] * q[3] - t[2] * q[2]); + dq->trans[2] = 0.5f * (-t[0] * q[3] + t[1] * q[0] + t[2] * q[1]); + dq->trans[3] = 0.5f * (t[0] * q[2] - t[1] * q[1] + t[2] * q[0]); } void dquat_to_mat4(float mat[][4], DualQuat *dq) { float len, *t, q0[4]; - + /* regular quaternion */ copy_qt_qt(q0, dq->quat); /* normalize */ - len= (float)sqrt(dot_qtqt(q0, q0)); + len = (float)sqrt(dot_qtqt(q0, q0)); if (len != 0.0f) - mul_qt_fl(q0, 1.0f/len); - + mul_qt_fl(q0, 1.0f / len); + /* rotation */ - quat_to_mat4(mat,q0); + quat_to_mat4(mat, q0); /* translation */ - t= dq->trans; - mat[3][0]= 2.0f*(-t[0]*q0[1] + t[1]*q0[0] - t[2]*q0[3] + t[3]*q0[2]); - mat[3][1]= 2.0f*(-t[0]*q0[2] + t[1]*q0[3] + t[2]*q0[0] - t[3]*q0[1]); - mat[3][2]= 2.0f*(-t[0]*q0[3] - t[1]*q0[2] + t[2]*q0[1] + t[3]*q0[0]); + t = dq->trans; + mat[3][0] = 2.0f * (-t[0] * q0[1] + t[1] * q0[0] - t[2] * q0[3] + t[3] * q0[2]); + mat[3][1] = 2.0f * (-t[0] * q0[2] + t[1] * q0[3] + t[2] * q0[0] - t[3] * q0[1]); + mat[3][2] = 2.0f * (-t[0] * q0[3] - t[1] * q0[2] + t[2] * q0[1] + t[3] * q0[0]); /* note: this does not handle scaling */ -} +} void add_weighted_dq_dq(DualQuat *dqsum, DualQuat *dq, float weight) { - int flipped= 0; + int flipped = 0; /* make sure we interpolate quats in the right direction */ if (dot_qtqt(dq->quat, dqsum->quat) < 0) { - flipped= 1; - weight= -weight; + flipped = 1; + weight = -weight; } /* interpolate rotation and translation */ - dqsum->quat[0] += weight*dq->quat[0]; - dqsum->quat[1] += weight*dq->quat[1]; - dqsum->quat[2] += weight*dq->quat[2]; - dqsum->quat[3] += weight*dq->quat[3]; + dqsum->quat[0] += weight * dq->quat[0]; + dqsum->quat[1] += weight * dq->quat[1]; + dqsum->quat[2] += weight * dq->quat[2]; + dqsum->quat[3] += weight * dq->quat[3]; - dqsum->trans[0] += weight*dq->trans[0]; - dqsum->trans[1] += weight*dq->trans[1]; - dqsum->trans[2] += weight*dq->trans[2]; - dqsum->trans[3] += weight*dq->trans[3]; + dqsum->trans[0] += weight * dq->trans[0]; + dqsum->trans[1] += weight * dq->trans[1]; + dqsum->trans[2] += weight * dq->trans[2]; + dqsum->trans[3] += weight * dq->trans[3]; /* interpolate scale - but only if needed */ if (dq->scale_weight) { float wmat[4][4]; - - if (flipped) /* we don't want negative weights for scaling */ - weight= -weight; - + + if (flipped) /* we don't want negative weights for scaling */ + weight = -weight; + copy_m4_m4(wmat, dq->scale); mul_m4_fl(wmat, weight); add_m4_m4m4(dqsum->scale, dqsum->scale, wmat); @@ -1542,14 +1592,14 @@ void add_weighted_dq_dq(DualQuat *dqsum, DualQuat *dq, float weight) void normalize_dq(DualQuat *dq, float totweight) { - float scale= 1.0f/totweight; + float scale = 1.0f / totweight; mul_qt_fl(dq->quat, scale); mul_qt_fl(dq->trans, scale); - + if (dq->scale_weight) { - float addweight= totweight - dq->scale_weight; - + float addweight = totweight - dq->scale_weight; + if (addweight) { dq->scale[0][0] += addweight; dq->scale[1][1] += addweight; @@ -1558,47 +1608,47 @@ void normalize_dq(DualQuat *dq, float totweight) } mul_m4_fl(dq->scale, scale); - dq->scale_weight= 1.0f; + dq->scale_weight = 1.0f; } } -void mul_v3m3_dq(float *co, float mat[][3],DualQuat *dq) -{ +void mul_v3m3_dq(float *co, float mat[][3], DualQuat *dq) +{ float M[3][3], t[3], scalemat[3][3], len2; - float w= dq->quat[0], x= dq->quat[1], y= dq->quat[2], z= dq->quat[3]; - float t0= dq->trans[0], t1= dq->trans[1], t2= dq->trans[2], t3= dq->trans[3]; - + float w = dq->quat[0], x = dq->quat[1], y = dq->quat[2], z = dq->quat[3]; + float t0 = dq->trans[0], t1 = dq->trans[1], t2 = dq->trans[2], t3 = dq->trans[3]; + /* rotation matrix */ - M[0][0]= w*w + x*x - y*y - z*z; - M[1][0]= 2*(x*y - w*z); - M[2][0]= 2*(x*z + w*y); - - M[0][1]= 2*(x*y + w*z); - M[1][1]= w*w + y*y - x*x - z*z; - M[2][1]= 2*(y*z - w*x); - - M[0][2]= 2*(x*z - w*y); - M[1][2]= 2*(y*z + w*x); - M[2][2]= w*w + z*z - x*x - y*y; - - len2= dot_qtqt(dq->quat, dq->quat); + M[0][0] = w * w + x * x - y * y - z * z; + M[1][0] = 2 * (x * y - w * z); + M[2][0] = 2 * (x * z + w * y); + + M[0][1] = 2 * (x * y + w * z); + M[1][1] = w * w + y * y - x * x - z * z; + M[2][1] = 2 * (y * z - w * x); + + M[0][2] = 2 * (x * z - w * y); + M[1][2] = 2 * (y * z + w * x); + M[2][2] = w * w + z * z - x * x - y * y; + + len2 = dot_qtqt(dq->quat, dq->quat); if (len2 > 0.0f) - len2= 1.0f/len2; - + len2 = 1.0f / len2; + /* translation */ - t[0]= 2*(-t0*x + w*t1 - t2*z + y*t3); - t[1]= 2*(-t0*y + t1*z - x*t3 + w*t2); - t[2]= 2*(-t0*z + x*t2 + w*t3 - t1*y); + t[0] = 2 * (-t0 * x + w * t1 - t2 * z + y * t3); + t[1] = 2 * (-t0 * y + t1 * z - x * t3 + w * t2); + t[2] = 2 * (-t0 * z + x * t2 + w * t3 - t1 * y); /* apply scaling */ if (dq->scale_weight) mul_m4_v3(dq->scale, co); - + /* apply rotation and translation */ mul_m3_v3(M, co); - co[0]= (co[0] + t[0])*len2; - co[1]= (co[1] + t[1])*len2; - co[2]= (co[2] + t[2])*len2; + co[0] = (co[0] + t[0]) * len2; + co[1] = (co[1] + t[1]) * len2; + co[2] = (co[2] + t[2]) * len2; /* compute crazyspace correction mat */ if (mat) { @@ -1619,85 +1669,85 @@ void copy_dq_dq(DualQuat *dq1, DualQuat *dq2) /* axis matches eTrackToAxis_Modes */ void quat_apply_track(float quat[4], short axis, short upflag) -{ +{ /* rotations are hard coded to match vec_to_quat */ - const float quat_track[][4]= {{0.70710676908493, 0.0, -0.70710676908493, 0.0}, /* pos-y90 */ - {0.5, 0.5, 0.5, 0.5}, /* Quaternion((1,0,0), radians(90)) * Quaternion((0,1,0), radians(90)) */ - {0.70710676908493, 0.0, 0.0, 0.70710676908493}, /* pos-z90 */ - {0.70710676908493, 0.0, 0.70710676908493, 0.0}, /* neg-y90 */ - {0.5, -0.5, -0.5, 0.5}, /* Quaternion((1,0,0), radians(-90)) * Quaternion((0,1,0), radians(-90)) */ - {-3.0908619663705394e-08, 0.70710676908493, 0.70710676908493, 3.0908619663705394e-08}}; /* no rotation */ + const float quat_track[][4] = { + {0.70710676908493, 0.0, -0.70710676908493, 0.0}, /* pos-y90 */ + {0.5, 0.5, 0.5, 0.5}, /* Quaternion((1,0,0), radians(90)) * Quaternion((0,1,0), radians(90)) */ + {0.70710676908493, 0.0, 0.0, 0.70710676908493}, /* pos-z90 */ + {0.70710676908493, 0.0, 0.70710676908493, 0.0}, /* neg-y90 */ + {0.5, -0.5, -0.5, 0.5}, /* Quaternion((1,0,0), radians(-90)) * Quaternion((0,1,0), radians(-90)) */ + {-3.0908619663705394e-08, 0.70710676908493, 0.70710676908493, 3.0908619663705394e-08}}; /* no rotation */ assert(axis >= 0 && axis <= 5); assert(upflag >= 0 && upflag <= 2); - + mul_qt_qtqt(quat, quat, quat_track[axis]); - if (axis>2) - axis= axis-3; + if (axis > 2) + axis = axis - 3; /* there are 2 possible up-axis for each axis used, the 'quat_track' applies so the first * up axis is used X->Y, Y->X, Z->X, if this first up axis isn used then rotate 90d * the strange bit shift below just find the low axis {X:Y, Y:X, Z:X} */ - if (upflag != (2-axis)>>1) { - float q[4]= {0.70710676908493, 0.0, 0.0, 0.0}; /* assign 90d rotation axis */ - q[axis+1] = ((axis==1)) ? 0.70710676908493 : -0.70710676908493; /* flip non Y axis */ + if (upflag != (2 - axis) >> 1) { + float q[4] = {0.70710676908493, 0.0, 0.0, 0.0}; /* assign 90d rotation axis */ + q[axis + 1] = ((axis == 1)) ? 0.70710676908493 : -0.70710676908493; /* flip non Y axis */ mul_qt_qtqt(quat, quat, q); } } - void vec_apply_track(float vec[3], short axis) { float tvec[3]; assert(axis >= 0 && axis <= 5); - + copy_v3_v3(tvec, vec); - switch(axis) { - case 0: /* pos-x */ - /* vec[0]= 0.0; */ - vec[1]= tvec[2]; - vec[2]= -tvec[1]; - break; - case 1: /* pos-y */ - /* vec[0]= tvec[0]; */ - /* vec[1]= 0.0; */ - /* vec[2]= tvec[2]; */ - break; - case 2: /* pos-z */ - /* vec[0]= tvec[0]; */ - /* vec[1]= tvec[1]; */ - // vec[2]= 0.0; */ - break; - case 3: /* neg-x */ - /* vec[0]= 0.0; */ - vec[1]= tvec[2]; - vec[2]= -tvec[1]; - break; - case 4: /* neg-y */ - vec[0]= -tvec[2]; - /* vec[1]= 0.0; */ - vec[2]= tvec[0]; - break; - case 5: /* neg-z */ - vec[0]= -tvec[0]; - vec[1]= -tvec[1]; - /* vec[2]= 0.0; */ - break; + switch (axis) { + case 0: /* pos-x */ + /* vec[0]= 0.0; */ + vec[1] = tvec[2]; + vec[2] = -tvec[1]; + break; + case 1: /* pos-y */ + /* vec[0]= tvec[0]; */ + /* vec[1]= 0.0; */ + /* vec[2]= tvec[2]; */ + break; + case 2: /* pos-z */ + /* vec[0]= tvec[0]; */ + /* vec[1]= tvec[1]; */ + // vec[2]= 0.0; */ + break; + case 3: /* neg-x */ + /* vec[0]= 0.0; */ + vec[1] = tvec[2]; + vec[2] = -tvec[1]; + break; + case 4: /* neg-y */ + vec[0] = -tvec[2]; + /* vec[1]= 0.0; */ + vec[2] = tvec[0]; + break; + case 5: /* neg-z */ + vec[0] = -tvec[0]; + vec[1] = -tvec[1]; + /* vec[2]= 0.0; */ + break; } } /* lens/angle conversion (radians) */ float focallength_to_fov(float focal_length, float sensor) { - return 2.0f * atanf((sensor/2.0f) / focal_length); + return 2.0f * atanf((sensor / 2.0f) / focal_length); } float fov_to_focallength(float hfov, float sensor) { - return (sensor/2.0f) / tanf(hfov * 0.5f); + return (sensor / 2.0f) / tanf(hfov * 0.5f); } /* 'mod_inline(-3,4)= 1', 'fmod(-3,4)= -3' */ @@ -1708,7 +1758,7 @@ static float mod_inline(float a, float b) float angle_wrap_rad(float angle) { - return mod_inline(angle + (float)M_PI, (float)M_PI*2.0f) - (float)M_PI; + return mod_inline(angle + (float)M_PI, (float)M_PI * 2.0f) - (float)M_PI; } float angle_wrap_deg(float angle) |