diff options
Diffstat (limited to 'source/blender/blenlib/intern/arithb.c')
| -rw-r--r-- | source/blender/blenlib/intern/arithb.c | 743 |
1 files changed, 601 insertions, 142 deletions
diff --git a/source/blender/blenlib/intern/arithb.c b/source/blender/blenlib/intern/arithb.c index 5b08586ffdb..d673df51f6c 100644 --- a/source/blender/blenlib/intern/arithb.c +++ b/source/blender/blenlib/intern/arithb.c @@ -641,17 +641,17 @@ void Mat4CpyMat4(float m1[][4], float m2[][4]) memcpy(m1, m2, 4*4*sizeof(float)); } -void Mat4SwapMat4(float *m1, float *m2) +void Mat4SwapMat4(float m1[][4], float m2[][4]) { float t; - int i; + int i, j; - for(i=0;i<16;i++) { - t= *m1; - *m1= *m2; - *m2= t; - m1++; - m2++; + for(i = 0; i < 4; i++) { + for (j = 0; j < 4; j++) { + t = m1[i][j]; + m1[i][j] = m2[i][j]; + m2[i][j] = t; + } } } @@ -881,6 +881,26 @@ void Mat3One(float m[][3]) m[2][0]= m[2][1]= 0.0; } +void Mat4Scale(float m[][4], float scale) +{ + + m[0][0]= m[1][1]= m[2][2]= scale; + m[3][3]= 1.0; + m[0][1]= m[0][2]= m[0][3]= 0.0; + m[1][0]= m[1][2]= m[1][3]= 0.0; + m[2][0]= m[2][1]= m[2][3]= 0.0; + m[3][0]= m[3][1]= m[3][2]= 0.0; +} + +void Mat3Scale(float m[][3], float scale) +{ + + m[0][0]= m[1][1]= m[2][2]= scale; + m[0][1]= m[0][2]= 0.0; + m[1][0]= m[1][2]= 0.0; + m[2][0]= m[2][1]= 0.0; +} + void Mat4MulVec( float mat[][4], int *vec) { int x,y; @@ -1435,22 +1455,6 @@ void RotationBetweenVectorsToQuat(float *q, float v1[3], float v2[3]) AxisAngleToQuat(q, axis, angle); } -void AxisAngleToQuat(float *q, float *axis, float angle) -{ - float nor[3]; - float si; - - VecCopyf(nor, axis); - Normalize(nor); - - angle /= 2; - si = (float)sin(angle); - q[0] = (float)cos(angle); - q[1] = nor[0] * si; - q[2] = nor[1] * si; - q[3] = nor[2] * si; -} - void AxisAngleToQuatd(float *q, float *axis, double angle) { double nor[3]; @@ -1659,7 +1663,7 @@ void VecUpMat3(float *vec, float mat[][3], short axis) } /* A & M Watt, Advanced animation and rendering techniques, 1992 ACM press */ -void QuatInterpolW(float *, float *, float *, float ); +void QuatInterpolW(float *, float *, float *, float ); // XXX why this? void QuatInterpolW(float *result, float *quat1, float *quat2, float t) { @@ -2856,6 +2860,241 @@ void MeanValueWeights(float v[][3], int n, float *co, float *w) /* ************ EULER *************** */ +/* Euler Rotation Order Code: + * was adapted from + ANSI C code from the article + "Euler Angle Conversion" + by Ken Shoemake, shoemake@graphics.cis.upenn.edu + in "Graphics Gems IV", Academic Press, 1994 + * for use in Blender + */ + +/* Type for rotation order info - see wiki for derivation details */ +typedef struct RotOrderInfo { + short i; /* first axis index */ + short j; /* second axis index */ + short k; /* third axis index */ + short parity; /* parity of axis permuation (even=0, odd=1) - 'n' in original code */ +} RotOrderInfo; + +/* Array of info for Rotation Order calculations + * WARNING: must be kept in same order as eEulerRotationOrders + */ +static RotOrderInfo rotOrders[]= { + /* i, j, k, n */ + {0, 1, 2, 0}, // XYZ + {0, 2, 1, 1}, // XZY + {1, 0, 2, 1}, // YXZ + {1, 2, 0, 0}, // YZX + {2, 0, 1, 0}, // ZXY + {2, 1, 0, 1} // ZYZ +}; + +/* 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) (((order)>=1) ? &rotOrders[(order)-1] : &rotOrders[0]) + +/* Construct quaternion from Euler angles (in radians). */ +void EulOToQuat(float e[3], short order, float q[4]) +{ + RotOrderInfo *R= GET_ROTATIONORDER_INFO(order); + short i=R->i, j=R->j, k=R->k; + double ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss; + double a[3]; + + ti = e[i]/2; tj = e[j]/2; th = e[k]/2; + + if (R->parity) e[j] = -e[j]; + + 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] = -q[j]; +} + +/* Convert quaternion to Euler angles (in radians). */ +void QuatToEulO(float q[4], float e[3], short order) +{ + float M[3][3]; + + QuatToMat3(q, M); + Mat3ToEulO(M, e, order); +} + +/* Construct 3x3 matrix from Euler angles (in radians). */ +void EulOToMat3(float e[3], short order, float M[3][3]) +{ + RotOrderInfo *R= GET_ROTATIONORDER_INFO(order); + short i=R->i, j=R->j, k=R->k; + 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]; + } + else { + 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; +} + +/* Construct 4x4 matrix from Euler angles (in radians). */ +void EulOToMat4(float e[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) */ + Mat3Ortho(m); + EulOToMat3(e, order, m); + Mat4CpyMat3(M, m); +} + +/* Convert 3x3 matrix to Euler angles (in radians). */ +void Mat3ToEulO(float M[3][3], float e[3], short order) +{ + RotOrderInfo *R= GET_ROTATIONORDER_INFO(order); + short i=R->i, j=R->j, k=R->k; + double cy = sqrt(M[i][i]*M[i][i] + M[i][j]*M[i][j]); + + if (cy > 16*FLT_EPSILON) { + e[i] = atan2(M[j][k], M[k][k]); + e[j] = atan2(-M[i][k], cy); + e[k] = atan2(M[i][j], M[i][i]); + } + else { + e[i] = atan2(-M[k][j], M[j][j]); + e[j] = atan2(-M[i][k], cy); + e[k] = 0; + } + + if (R->parity) { + e[0] = -e[0]; + e[1] = -e[1]; + e[2] = -e[2]; + } +} + +/* Convert 4x4 matrix to Euler angles (in radians). */ +void Mat4ToEulO(float M[4][4], float e[3], short order) +{ + float m[3][3]; + + /* for now, we'll just do this the slow way (i.e. copying matrices) */ + Mat3CpyMat4(m, M); + Mat3Ortho(m); + Mat3ToEulO(m, e, order); +} + +/* 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->i, j=R->j, k=R->k; + float m[3][3]; + double cy; + + /* process the matrix first */ + Mat3CpyMat3(m, M); + Mat3Ortho(m); + + cy= sqrt(m[i][i]*m[i][i] + m[i][j]*m[i][j]); + + if (cy > 16*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; + + VecCopyf(e2, e1); + } + + if (R->parity) { + e1[0] = -e1[0]; + e1[1] = -e1[1]; + e1[2] = -e1[2]; + + e2[0] = -e2[0]; + e2[1] = -e2[1]; + e2[2] = -e2[2]; + } +} + +/* uses 2 methods to retrieve eulers, and picks the closest */ +void Mat3ToCompatibleEulO(float mat[3][3], float eul[3], float oldrot[3], short order) +{ + float eul1[3], eul2[3]; + float d1, d2; + + mat3_to_eulo2(mat, eul1, eul2, order); + + 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]); + + /* return best, which is just the one with lowest difference */ + if (d1 > d2) + VecCopyf(eul, eul2); + else + VecCopyf(eul, eul1); +} + +/* rotate the given euler by the given angle on the specified axis */ +// NOTE: is this safe to do with different axis orders? +void eulerO_rot(float beul[3], float ang, char axis, short order) +{ + float eul[3], mat1[3][3], mat2[3][3], totmat[3][3]; + + 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; + + EulOToMat3(eul, order, mat1); + EulOToMat3(beul, order, mat2); + + Mat3MulMat3(totmat, mat2, mat1); + + Mat3ToEulO(totmat, beul, order); +} + +/* ************ EULER (old XYZ) *************** */ + +/* XYZ order */ void EulToMat3( float *eul, float mat[][3]) { double ci, cj, ch, si, sj, sh, cc, cs, sc, ss; @@ -2883,6 +3122,7 @@ void EulToMat3( float *eul, float mat[][3]) } +/* XYZ order */ void EulToMat4( float *eul,float mat[][4]) { double ci, cj, ch, si, sj, sh, cc, cs, sc, ss; @@ -2914,6 +3154,7 @@ void EulToMat4( float *eul,float mat[][4]) } /* returns two euler calculation methods, so we can pick the best */ +/* XYZ order */ static void mat3_to_eul2(float tmat[][3], float *eul1, float *eul2) { float cy, quat[4], mat[3][3]; @@ -2944,6 +3185,7 @@ static void mat3_to_eul2(float tmat[][3], float *eul1, float *eul2) } } +/* XYZ order */ void Mat3ToEul(float tmat[][3], float *eul) { float eul1[3], eul2[3]; @@ -2959,6 +3201,7 @@ void Mat3ToEul(float tmat[][3], float *eul) } } +/* XYZ order */ void Mat4ToEul(float tmat[][4], float *eul) { float tempMat[3][3]; @@ -2968,6 +3211,7 @@ void Mat4ToEul(float tmat[][4], float *eul) Mat3ToEul(tempMat, eul); } +/* XYZ order */ void QuatToEul(float *quat, float *eul) { float mat[3][3]; @@ -2976,7 +3220,7 @@ void QuatToEul(float *quat, float *eul) Mat3ToEul(mat, eul); } - +/* XYZ order */ void EulToQuat(float *eul, float *quat) { float ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss; @@ -2992,6 +3236,261 @@ void EulToQuat(float *eul, float *quat) quat[3] = cj*cs - sj*sc; } +/* XYZ order */ +void euler_rot(float *beul, float ang, char axis) +{ + float eul[3], mat1[3][3], mat2[3][3], totmat[3][3]; + + 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; + + EulToMat3(eul, mat1); + EulToMat3(beul, mat2); + + Mat3MulMat3(totmat, mat2, mat1); + + Mat3ToEul(totmat, beul); + +} + +/* exported to transform.c */ +/* order independent! */ +void compatible_eul(float *eul, float *oldrot) +{ + 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]; + + 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]; + } + 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]; + } + 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]; + } + + /* 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.0) 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.0) 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.0) 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 + /* calc again */ + 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; + + } + 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]; + } + 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; + } +#endif +} + +/* uses 2 methods to retrieve eulers, and picks the closest */ +/* XYZ order */ +void Mat3ToCompatibleEul(float mat[][3], float *eul, float *oldrot) +{ + 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]); + + /* return best, which is just the one with lowest difference */ + if( d1 > d2) { + VecCopyf(eul, eul2); + } + else { + VecCopyf(eul, eul1); + } + +} + +/* ************ AXIS ANGLE *************** */ + +/* Axis angle to Quaternions */ +void AxisAngleToQuat(float q[4], float axis[3], float angle) +{ + float nor[3]; + float si; + + VecCopyf(nor, axis); + Normalize(nor); + + angle /= 2; + si = (float)sin(angle); + q[0] = (float)cos(angle); + q[1] = nor[0] * si; + q[2] = nor[1] * si; + q[3] = nor[2] * si; +} + +/* Quaternions to Axis Angle */ +void QuatToAxisAngle(float q[4], float axis[3], float *angle) +{ + float ha, si; + + /* calculate angle/2, and sin(angle/2) */ + ha= (float)acos(q[0]); + si= (float)sin(ha); + + /* from half-angle to angle */ + *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; +} + +/* Axis Angle to Euler Rotation */ +void AxisAngleToEulO(float axis[3], float angle, float eul[3], short order) +{ + float q[4]; + + /* use quaternions as intermediate representation for now... */ + AxisAngleToQuat(q, axis, angle); + QuatToEulO(q, eul, order); +} + +/* Euler Rotation to Axis Angle */ +void EulOToAxisAngle(float eul[3], short order, float axis[3], float *angle) +{ + float q[4]; + + /* use quaternions as intermediate representation for now... */ + EulOToQuat(eul, order, q); + QuatToAxisAngle(q, axis, angle); +} + +/* axis angle to 3x3 matrix - safer version (normalisation of axis performed) */ +void AxisAngleToMat3(float axis[3], float angle, float mat[3][3]) +{ + float nor[3], nsi[3], co, si, ico; + + /* normalise the axis first (to remove unwanted scaling) */ + VecCopyf(nor, axis); + Normalize(nor); + + /* 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; + + 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]; + mat[1][0] = ((nor[0] * nor[1]) * ico) - nsi[2]; + mat[1][1] = ((nor[1] * nor[1]) * ico) + co; + mat[1][2] = ((nor[1] * nor[2]) * ico) + nsi[0]; + mat[2][0] = ((nor[0] * nor[2]) * ico) + nsi[1]; + mat[2][1] = ((nor[1] * nor[2]) * ico) - nsi[0]; + mat[2][2] = ((nor[2] * nor[2]) * ico) + co; +} + +/* axis angle to 4x4 matrix - safer version (normalisation of axis performed) */ +void AxisAngleToMat4(float axis[3], float angle, float mat[4][4]) +{ + float tmat[3][3]; + + AxisAngleToMat3(axis, angle, tmat); + Mat4One(mat); + Mat4CpyMat3(mat, tmat); +} + +/* 3x3 matrix to axis angle (see Mat4ToVecRot too) */ +void Mat3ToAxisAngle(float mat[3][3], float axis[3], float *angle) +{ + float q[4]; + + /* use quaternions as intermediate representation */ + // TODO: it would be nicer to go straight there... + Mat3ToQuat(mat, q); + QuatToAxisAngle(q, axis, angle); +} + +/* 4x4 matrix to axis angle (see Mat4ToVecRot too) */ +void Mat4ToAxisAngle(float mat[4][4], float axis[3], float *angle) +{ + float q[4]; + + /* use quaternions as intermediate representation */ + // TODO: it would be nicer to go straight there... + Mat4ToQuat(mat, q); + QuatToAxisAngle(q, axis, angle); +} + +/* ************ AXIS ANGLE (unchecked) *************** */ +// TODO: the following calls should probably be depreceated sometime + +/* 3x3 matrix to axis angle */ +void Mat3ToVecRot(float mat[3][3], float axis[3], float *angle) +{ + float q[4]; + + /* use quaternions as intermediate representation */ + // TODO: it would be nicer to go straight there... + Mat3ToQuat(mat, q); + QuatToAxisAngle(q, axis, angle); +} + +/* 4x4 matrix to axis angle */ +void Mat4ToVecRot(float mat[4][4], float axis[3], float *angle) +{ + float q[4]; + + /* use quaternions as intermediate representation */ + // TODO: it would be nicer to go straight there... + Mat4ToQuat(mat, q); + QuatToAxisAngle(q, axis, angle); +} + +/* axis angle to 3x3 matrix */ void VecRotToMat3(float *vec, float phi, float mat[][3]) { /* rotation of phi radials around vec */ @@ -3015,9 +3514,9 @@ void VecRotToMat3(float *vec, float phi, float mat[][3]) 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 VecRotToMat4(float *vec, float phi, float mat[][4]) { float tmat[3][3]; @@ -3027,6 +3526,7 @@ void VecRotToMat4(float *vec, float phi, float mat[][4]) Mat4CpyMat3(mat, tmat); } +/* axis angle to quaternion */ void VecRotToQuat(float *vec, float phi, float *quat) { /* rotation of phi radials around vec */ @@ -3048,6 +3548,43 @@ void VecRotToQuat(float *vec, float phi, float *quat) } } +/* ************ VECTORS *************** */ + +/* Returns a vector bisecting the angle at v2 formed by v1, v2 and v3 */ +void VecBisect3(float *out, float *v1, float *v2, float *v3) +{ + float d_12[3], d_23[3]; + VecSubf(d_12, v2, v1); + VecSubf(d_23, v3, v2); + Normalize(d_12); + Normalize(d_23); + VecAddf(out, d_12, d_23); + Normalize(out); +} + +/* Returns a reflection vector from a vector and a normal vector +reflect = vec - ((2 * DotVecs(vec, mirror)) * mirror) +*/ +void VecReflect(float *out, float *v1, float *v2) +{ + float vec[3], normal[3]; + float reflect[3] = {0.0f, 0.0f, 0.0f}; + float dot2; + + VecCopyf(vec, v1); + VecCopyf(normal, v2); + + Normalize(normal); + + dot2 = 2 * Inpf(vec, normal); + + reflect[0] = vec[0] - (dot2 * normal[0]); + reflect[1] = vec[1] - (dot2 * normal[1]); + reflect[2] = vec[2] - (dot2 * normal[2]); + + VecCopyf(out, reflect); +} + /* Return the angle in degrees between vecs 1-2 and 2-3 in degrees If v1 is a shoulder, v2 is the elbow and v3 is the hand, this would return the angle at the elbow */ @@ -3123,111 +3660,6 @@ float NormalizedVecAngle2_2D(float *v1, float *v2) return 2.0f*(float)saasin(Vec2Lenf(v2, v1)/2.0f); } -void euler_rot(float *beul, float ang, char axis) -{ - float eul[3], mat1[3][3], mat2[3][3], totmat[3][3]; - - 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; - - EulToMat3(eul, mat1); - EulToMat3(beul, mat2); - - Mat3MulMat3(totmat, mat2, mat1); - - Mat3ToEul(totmat, beul); - -} - -/* exported to transform.c */ -void compatible_eul(float *eul, float *oldrot) -{ - 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]; - - 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]; - } - 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]; - } - 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]; - } - - /* 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.0) 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.0) 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.0) 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 - /* calc again */ - 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; - - } - 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]; - } - 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; - } -#endif -} - -/* uses 2 methods to retrieve eulers, and picks the closest */ -void Mat3ToCompatibleEul(float mat[][3], float *eul, float *oldrot) -{ - 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]); - - /* return best, which is just the one with lowest difference */ - if( d1 > d2) { - VecCopyf(eul, eul2); - } - else { - VecCopyf(eul, eul1); - } - -} - /* ******************************************** */ void SizeToMat3( float *size, float mat[][3]) @@ -3710,19 +4142,19 @@ void spheremap(float x, float y, float z, float *u, float *v) /* proposed api by ton and zr, not used yet */ #if 0 /* ***************** m1 = m2 ***************** */ -void cpy_m3_m3(float m1[][3], float m2[][3]) +static void cpy_m3_m3(float m1[][3], float m2[][3]) { memcpy(m1[0], m2[0], 9*sizeof(float)); } /* ***************** m1 = m2 ***************** */ -void cpy_m4_m4(float m1[][4], float m2[][4]) +static void cpy_m4_m4(float m1[][4], float m2[][4]) { memcpy(m1[0], m2[0], 16*sizeof(float)); } /* ***************** identity matrix ***************** */ -void ident_m4(float m[][4]) +static void ident_m4(float m[][4]) { m[0][0]= m[1][1]= m[2][2]= m[3][3]= 1.0; @@ -3733,7 +4165,7 @@ void ident_m4(float m[][4]) } /* ***************** m1 = m2 (pre) * m3 (post) ***************** */ -void mul_m3_m3m3(float m1[][3], float m2[][3], float m3[][3]) +static void mul_m3_m3m3(float m1[][3], float m2[][3], float m3[][3]) { float m[3][3]; @@ -3753,7 +4185,7 @@ void mul_m3_m3m3(float m1[][3], float m2[][3], float m3[][3]) } /* ***************** m1 = m2 (pre) * m3 (post) ***************** */ -void mul_m4_m4m4(float m1[][4], float m2[][4], float m3[][4]) +static void mul_m4_m4m4(float m1[][4], float m2[][4], float m3[][4]) { float m[4][4]; @@ -3781,7 +4213,7 @@ void mul_m4_m4m4(float m1[][4], float m2[][4], float m3[][4]) } /* ***************** m1 = inverse(m2) ***************** */ -void inv_m3_m3(float m1[][3], float m2[][3]) +static void inv_m3_m3(float m1[][3], float m2[][3]) { short a,b; float det; @@ -3804,7 +4236,7 @@ void inv_m3_m3(float m1[][3], float m2[][3]) } /* ***************** m1 = inverse(m2) ***************** */ -int inv_m4_m4(float inverse[][4], float mat[][4]) +static int inv_m4_m4(float inverse[][4], float mat[][4]) { int i, j, k; double temp; @@ -3857,7 +4289,7 @@ int inv_m4_m4(float inverse[][4], float mat[][4]) } /* ***************** v1 = v2 * mat ***************** */ -void mul_v3_v3m4(float *v1, float *v2, float mat[][4]) +static void mul_v3_v3m4(float *v1, float *v2, float mat[][4]) { float x, y; @@ -4750,7 +5182,8 @@ float PdistVL3Dfl(float *v1, float *v2, float *v3) /* make a 4x4 matrix out of 3 transform components */ /* matrices are made in the order: scale * rot * loc */ -void LocEulSizeToMat4(float mat[][4], float loc[3], float eul[3], float size[3]) +// TODO: need to have a version that allows for rotation order... +void LocEulSizeToMat4(float mat[4][4], float loc[3], float eul[3], float size[3]) { float rmat[3][3], smat[3][3], tmat[3][3]; @@ -4773,7 +5206,31 @@ void LocEulSizeToMat4(float mat[][4], float loc[3], float eul[3], float size[3]) /* make a 4x4 matrix out of 3 transform components */ /* matrices are made in the order: scale * rot * loc */ -void LocQuatSizeToMat4(float mat[][4], float loc[3], float quat[4], float size[3]) +void LocEulOSizeToMat4(float mat[4][4], float loc[3], float eul[3], float size[3], short rotOrder) +{ + float rmat[3][3], smat[3][3], tmat[3][3]; + + /* initialise new matrix */ + Mat4One(mat); + + /* make rotation + scaling part */ + EulOToMat3(eul, rotOrder, rmat); + SizeToMat3(size, smat); + Mat3MulMat3(tmat, rmat, smat); + + /* copy rot/scale part to output matrix*/ + Mat4CpyMat3(mat, tmat); + + /* copy location to matrix */ + mat[3][0] = loc[0]; + mat[3][1] = loc[1]; + mat[3][2] = loc[2]; +} + + +/* make a 4x4 matrix out of 3 transform components */ +/* matrices are made in the order: scale * rot * loc */ +void LocQuatSizeToMat4(float mat[4][4], float loc[3], float quat[4], float size[3]) { float rmat[3][3], smat[3][3], tmat[3][3]; @@ -4794,6 +5251,8 @@ void LocQuatSizeToMat4(float mat[][4], float loc[3], float quat[4], float size[3 mat[3][2] = loc[2]; } +/********************************************************/ + /* Tangents */ /* For normal map tangents we need to detect uv boundaries, and only average |