diff options
Diffstat (limited to 'source/blender/blenlib/intern/arithb.c')
| -rw-r--r-- | source/blender/blenlib/intern/arithb.c | 879 |
1 files changed, 683 insertions, 196 deletions
diff --git a/source/blender/blenlib/intern/arithb.c b/source/blender/blenlib/intern/arithb.c index f111e94a141..26bbbf040f3 100644 --- a/source/blender/blenlib/intern/arithb.c +++ b/source/blender/blenlib/intern/arithb.c @@ -92,21 +92,41 @@ float sasqrt(float fac) return (float)sqrt(fac); } +float saacosf(float fac) +{ + if(fac<= -1.0f) return (float)M_PI; + else if(fac>=1.0f) return 0.0f; + else return (float)acosf(fac); +} + +float saasinf(float fac) +{ + if(fac<= -1.0f) return (float)-M_PI/2.0f; + else if(fac>=1.0f) return (float)M_PI/2.0f; + else return (float)asinf(fac); +} + +float sasqrtf(float fac) +{ + if(fac<=0.0) return 0.0; + return (float)sqrtf(fac); +} + float Normalize(float *n) { float d; d= n[0]*n[0]+n[1]*n[1]+n[2]*n[2]; /* A larger value causes normalize errors in a scaled down models with camera xtreme close */ - if(d>1.0e-35F) { + if(d>1.0e-35f) { d= (float)sqrt(d); n[0]/=d; n[1]/=d; n[2]/=d; } else { - n[0]=n[1]=n[2]= 0.0; - d= 0.0; + n[0]=n[1]=n[2]= 0.0f; + d= 0.0f; } return d; } @@ -594,17 +614,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; + } } } @@ -834,6 +854,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; @@ -1388,22 +1428,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 vectoquat(float *vec, short axis, short upflag, float *q) { float q2[4], nor[3], *fp, mat[3][3], angle, si, co, x2, y2, z2, len1; @@ -1590,7 +1614,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) { @@ -2156,23 +2180,47 @@ void VecSubf(float *v, float *v1, float *v2) v[2]= v1[2]- v2[2]; } -void VecLerpf(float *target, float *a, float *b, float t) +void VecMulVecf(float *v, float *v1, float *v2) +{ + v[0] = v1[0] * v2[0]; + v[1] = v1[1] * v2[1]; + v[2] = v1[2] * v2[2]; +} + +void VecLerpf(float *target, const float *a, const float *b, const float t) { - float s = 1.0f-t; + const float s = 1.0f-t; target[0]= s*a[0] + t*b[0]; target[1]= s*a[1] + t*b[1]; target[2]= s*a[2] + t*b[2]; } -void Vec2Lerpf(float *target, float *a, float *b, float t) +void Vec2Lerpf(float *target, const float *a, const float *b, const float t) { - float s = 1.0f-t; + const float s = 1.0f-t; target[0]= s*a[0] + t*b[0]; target[1]= s*a[1] + t*b[1]; } +/* weight 3 vectors, (VecWeightf in 2.4x) + * 'w' must be unit length but is not a vector, just 3 weights */ +void VecLerp3f(float p[3], const float v1[3], const float v2[3], const float v3[3], const float w[3]) +{ + p[0] = v1[0]*w[0] + v2[0]*w[1] + v3[0]*w[2]; + p[1] = v1[1]*w[0] + v2[1]*w[1] + v3[1]*w[2]; + p[2] = v1[2]*w[0] + v2[2]*w[1] + v3[2]*w[2]; +} + +/* weight 3 2D vectors, (Vec2Weightf in 2.4x) + * 'w' must be unit length but is not a vector, just 3 weights */ +void Vec2Lerp3f(float p[2], const float v1[2], const float v2[2], const float v3[2], const float w[3]) +{ + p[0] = v1[0]*w[0] + v2[0]*w[1] + v3[0]*w[2]; + p[1] = v1[1]*w[0] + v2[1]*w[1] + v3[1]*w[2]; +} + void VecMidf(float *v, float *v1, float *v2) { v[0]= 0.5f*(v1[0]+ v2[0]); @@ -2197,25 +2245,23 @@ void VecNegf(float *v1) void VecOrthoBasisf(float *v, float *v1, float *v2) { - float f = (float)sqrt(v[0]*v[0] + v[1]*v[1]); + const float f = (float)sqrt(v[0]*v[0] + v[1]*v[1]); if (f < 1e-35f) { // degenerate case - v1[0] = 0.0f; v1[1] = 1.0f; v1[2] = 0.0f; - if (v[2] > 0.0f) { - v2[0] = 1.0f; v2[1] = v2[2] = 0.0f; - } - else { - v2[0] = -1.0f; v2[1] = v2[2] = 0.0f; - } + v1[0] = (v[2] < 0.0f) ? -1.0f : 1.0f; + v1[1] = v1[2] = v2[0] = v2[2] = 0.0f; + v2[1] = 1.0f; } else { - f = 1.0f/f; - v1[0] = v[1]*f; - v1[1] = -v[0]*f; - v1[2] = 0.0f; + const float d= 1.0f/f; - Crossf(v2, v, v1); + v1[0] = v[1]*d; + v1[1] = -v[0]*d; + v1[2] = 0.0f; + v2[0] = -v[2]*v1[1]; + v2[1] = v[2]*v1[0]; + v2[2] = v[0]*v1[1] - v[1]*v1[0]; } } @@ -2782,6 +2828,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; @@ -2809,6 +3090,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; @@ -2840,6 +3122,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]; @@ -2870,6 +3153,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]; @@ -2885,6 +3169,7 @@ void Mat3ToEul(float tmat[][3], float *eul) } } +/* XYZ order */ void Mat4ToEul(float tmat[][4], float *eul) { float tempMat[3][3]; @@ -2894,6 +3179,7 @@ void Mat4ToEul(float tmat[][4], float *eul) Mat3ToEul(tempMat, eul); } +/* XYZ order */ void QuatToEul(float *quat, float *eul) { float mat[3][3]; @@ -2902,7 +3188,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; @@ -2918,6 +3204,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 */ @@ -2941,9 +3482,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]; @@ -2953,6 +3494,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 */ @@ -2974,6 +3516,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 */ @@ -2986,10 +3565,10 @@ float VecAngle3(float *v1, float *v2, float *v3) Normalize(vec1); Normalize(vec2); - return NormalizedVecAngle2(vec1, vec2) * (float)(180.0/M_PI); + return NormalizedVecAngle2(vec1, vec2); } -float VecAngle3_2D(float *v1, float *v2, float *v3) +float Vec2Angle3(float *v1, float *v2, float *v3) { float vec1[2], vec2[2]; @@ -3002,7 +3581,7 @@ float VecAngle3_2D(float *v1, float *v2, float *v3) Normalize2(vec1); Normalize2(vec2); - return NormalizedVecAngle2_2D(vec1, vec2) * (float)(180.0/M_PI); + return NormalizedVecAngle2_2D(vec1, vec2); } /* Return the shortest angle in degrees between the 2 vectors */ @@ -3015,7 +3594,7 @@ float VecAngle2(float *v1, float *v2) Normalize(vec1); Normalize(vec2); - return NormalizedVecAngle2(vec1, vec2)* (float)(180.0/M_PI); + return NormalizedVecAngle2(vec1, vec2); } float NormalizedVecAngle2(float *v1, float *v2) @@ -3049,111 +3628,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]) @@ -3316,7 +3790,7 @@ float Normalize2(float *n) d= n[0]*n[0]+n[1]*n[1]; - if(d>1.0e-35F) { + if(d>1.0e-35f) { d= (float)sqrt(d); n[0]/=d; n[1]/=d; @@ -3334,15 +3808,15 @@ void hsv_to_rgb(float h, float s, float v, float *r, float *g, float *b) h *= 360.0f; - if(s==0.0) { + if(s==0.0f) { *r = v; *g = v; *b = v; } else { - if(h==360) h = 0; + if(h== 360.0f) h = 0.0f; - h /= 60; + h /= 60.0f; i = (int)floor(h); f = h - i; p = v*(1.0f-s); @@ -3542,33 +4016,10 @@ int constrain_rgb(float *r, float *g, float *b) if (w > 0) { *r += w; *g += w; *b += w; - return 1; /* Colour modified to fit RGB gamut */ + return 1; /* Color modified to fit RGB gamut */ } - return 0; /* Colour within RGB gamut */ -} - -/*Transform linear RGB values to nonlinear RGB values. Rec. - 709 is ITU-R Recommendation BT. 709 (1990) ``Basic - Parameter Values for the HDTV Standard for the Studio and - for International Programme Exchange'', formerly CCIR Rec. - 709.*/ -static void gamma_correct(float *c) -{ - /* Rec. 709 gamma correction. */ - float cc = 0.018f; - - if (*c < cc) - *c *= ((1.099f * (float)pow(cc, 0.45)) - 0.099f) / cc; - else - *c = (1.099f * (float)pow(*c, 0.45)) - 0.099f; -} - -void gamma_correct_rgb(float *r, float *g, float *b) -{ - gamma_correct(r); - gamma_correct(g); - gamma_correct(b); + return 0; /* Color within RGB gamut */ } @@ -3659,19 +4110,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; @@ -3682,7 +4133,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]; @@ -3702,7 +4153,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]; @@ -3730,7 +4181,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; @@ -3753,7 +4204,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; @@ -3806,7 +4257,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; @@ -4281,7 +4732,7 @@ int LineIntersectLine(float v1[3], float v2[3], float v3[3], float v4[3], float VecSubf(c, v3t, v1); VecSubf(a, v2, v1); - VecSubf(b, v4t, v3); + VecSubf(b, v4t, v3t); Crossf(ab, a, b); Crossf(cb, c, b); @@ -4389,6 +4840,15 @@ static float lambda_cp_line(float p[3], float l1[3], float l2[3]) } #endif +/* useful to calculate an even width shell, by taking the angle between 2 planes. + * The return value is a scale on the offset. + * no angle between planes is 1.0, as the angle between the 2 planes approches 180d + * the distance gets very high, 180d would be inf, but this case isn't valid */ +float AngleToLength(const float angle) +{ + return (angle < SMALL_NUMBER) ? 1.0f : fabsf(1.0f / cosf(angle * (M_PI/180.0f))); +} + /* Similar to LineIntersectsTriangleUV, except it operates on a quad and in 2d, assumes point is in quad */ void PointInQuad2DUV(float v0[2], float v1[2], float v2[2], float v3[2], float pt[2], float *uv) { @@ -4699,7 +5159,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]; @@ -4722,7 +5183,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]; @@ -4743,6 +5228,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 |