Commit b92b2ae0 by slevy

### Use isfinite() rather than finite().

`Add tfm2euler() and tfm2xyzeuler(), translated from the perl.`
parent 08bc8f1c
 ... @@ -363,7 +363,7 @@ float tfm2iquat( Point *iquat, CONST Matrix *T ) ... @@ -363,7 +363,7 @@ float tfm2iquat( Point *iquat, CONST Matrix *T ) } } } } mag = vlength(&axis); mag = vlength(&axis); if(!finite(mag)) { if(!isfinite(mag)) { fprintf(stderr, "Yikes, tfm2quat() yields NaN?\n"); fprintf(stderr, "Yikes, tfm2quat() yields NaN?\n"); } } /* The imaginary part is a vector pointing along the axis of rotation, /* The imaginary part is a vector pointing along the axis of rotation, ... @@ -383,11 +383,12 @@ float mdet3( CONST Matrix *T ) ... @@ -383,11 +383,12 @@ float mdet3( CONST Matrix *T ) return vdot( &v01, (CONST Point *)&T->m[8] ); return vdot( &v01, (CONST Point *)&T->m[8] ); } } #define Tij(i,j) T->m[(i)*4+(j)] void tfm2quat( Quat *quat, CONST Matrix *T ) void tfm2quat( Quat *quat, CONST Matrix *T ) { { float ww, xx, yy, zz; /* i.e. w^2, x^2, etc. */ float ww, xx, yy, zz; /* i.e. w^2, x^2, etc. */ float w, x, y, z, max; float w, x, y, z, max; float s = vlength( (Point *)&T->m[0*4+0] ); float s = sqrt( Tij(0,0)*Tij(0,0) + Tij(0,1)*Tij(0,1) + Tij(0,2)*Tij(0,2) ); int best = 0; int best = 0; /* A rotation matrix is /* A rotation matrix is ... @@ -397,17 +398,15 @@ void tfm2quat( Quat *quat, CONST Matrix *T ) ... @@ -397,17 +398,15 @@ void tfm2quat( Quat *quat, CONST Matrix *T ) * and * and * ww+xx+yy+zz = ss * ww+xx+yy+zz = ss */ */ if(s > 0) { if(s == 0) { /* OK */ } else { quat->q[0] = 1; quat->q[0] = 1; quat->q[1] = quat->q[2] = quat->q[3] = 0; quat->q[1] = quat->q[2] = quat->q[3] = 0; return; return; } } ww = (s + T->m[0*4+0] + T->m[1*4+1] + T->m[2*4+2]); /* 4 * w^2 */ ww = (s + Tij(0,0) + Tij(1,1) + Tij(2,2)); /* 4 * w^2 */ xx = (s + T->m[0*4+0] - T->m[1*4+1] - T->m[2*4+2]); xx = (s + Tij(0,0) - Tij(1,1) - Tij(2,2)); yy = (s - T->m[0*4+0] + T->m[1*4+1] - T->m[2*4+2]); yy = (s - Tij(0,0) + Tij(1,1) - Tij(2,2)); zz = (s - T->m[0*4+0] - T->m[1*4+1] + T->m[2*4+2]); zz = (s - Tij(0,0) - Tij(1,1) + Tij(2,2)); max = ww; max = ww; if(max < xx) max = xx, best = 1; if(max < xx) max = xx, best = 1; ... @@ -416,45 +415,46 @@ void tfm2quat( Quat *quat, CONST Matrix *T ) ... @@ -416,45 +415,46 @@ void tfm2quat( Quat *quat, CONST Matrix *T ) switch(best) { switch(best) { case 0: /* ww == max */ case 0: /* ww == max */ w = sqrtf(ww) * 2; /* 4w */ w = sqrt(ww) * 2; /* 4w */ x = (T->m[2*4+1] - T->m[1*4+2]) / w; /* 4wx/4w */ x = (Tij(2,1) - Tij(1,2)) / w; /* 4wx/4w */ y = (T->m[0*4+2] - T->m[2*4+0]) / w; /* 4wy/4w */ y = (Tij(0,2) - Tij(2,0)) / w; /* 4wy/4w */ z = (T->m[1*4+0] - T->m[0*4+1]) / w; /* 4wz/4w */ z = (Tij(1,0) - Tij(0,1)) / w; /* 4wz/4w */ w *= .25f; /* w */ w *= .25f; /* w */ break; break; case 1: /* xx == max */ case 1: /* xx == max */ x = sqrtf(xx) * 2; /* 4x */ x = sqrt(xx) * 2; /* 4x */ w = (T->m[2*4+1] - T->m[1*4+2]) / x; /* 4wx/4x */ w = (Tij(2,1) - Tij(1,2)) / x; /* 4wx/4x */ y = (T->m[1*4+0] + T->m[0*4+1]) / x; /* 4xy/4x */ y = (Tij(1,0) + Tij(0,1)) / x; /* 4xy/4x */ z = (T->m[0*4+2] + T->m[2*4+0]) / x; /* 4xz/4x */ z = (Tij(0,2) + Tij(2,0)) / x; /* 4xz/4x */ x *= .25f; /* x */ x *= .25f; /* x */ break; break; case 2: /* yy == max */ case 2: /* yy == max */ y = sqrtf(yy) * 2; /* 4y */ y = sqrt(yy) * 2; /* 4y */ w = (T->m[0*4+2] - T->m[2*4+0]) / y; /* 4wy/4y */ w = (Tij(0,2) - Tij(2,0)) / y; /* 4wy/4y */ x = (T->m[1*4+0] + T->m[0*4+1]) / y; /* 4xy/4y */ x = (Tij(1,0) + Tij(0,1)) / y; /* 4xy/4y */ z = (T->m[2*4+1] + T->m[1*4+2]) / y; /* 4yz/4y */ z = (Tij(2,1) + Tij(1,2)) / y; /* 4yz/4y */ y *= .25f; /* y */ y *= .25f; /* y */ break; break; default: /* zz == max */ default: /* zz == max */ z = sqrtf(zz) * 2; /* 4z */ z = sqrt(zz) * 2; /* 4z */ w = (T->m[1*4+0] - T->m[0*4+1]) / z; /* 4wz/4z */ w = (Tij(1,0) - Tij(0,1)) / z; /* 4wz/4z */ x = (T->m[0*4+2] + T->m[2*4+0]) / z; /* 4xz/4z */ x = (Tij(0,2) + Tij(2,0)) / z; /* 4xz/4z */ y = (T->m[2*4+1] + T->m[1*4+2]) / z; /* 4yz/4z */ y = (Tij(2,1) + Tij(1,2)) / z; /* 4yz/4z */ z *= .25f; /* z */ z *= .25f; /* z */ break; break; } } s = sqrtf(s); s = sqrt(s); quat->q[0] = -w/s; quat->q[0] = -w/s; quat->q[1] = x/s; quat->q[1] = x/s; quat->q[2] = y/s; quat->q[2] = y/s; quat->q[3] = z/s; quat->q[3] = z/s; } } #undef Tij void quat2tfm( register Matrix *dst, CONST Quat *quat ) void quat2tfm( register Matrix *dst, CONST Quat *quat ) { { ... @@ -638,13 +638,132 @@ void iquat_lerp( Point *qdst, float frac, CONST Point *qfrom, CONST Point *qto ) ... @@ -638,13 +638,132 @@ void iquat_lerp( Point *qdst, float frac, CONST Point *qfrom, CONST Point *qto ) rdst = (1-frac)*rfrom + frac*rto; rdst = (1-frac)*rfrom + frac*rto; vlerp( &dst, frac, qfrom, &tto ); vlerp( &dst, frac, qfrom, &tto ); s = 1/sqrtf(rdst*rdst + VDOT(&dst,&dst)); s = 1/sqrtf(rdst*rdst + VDOT(&dst,&dst)); if(!finite(s)) { if(!isfinite(s)) { fprintf(stderr, "Yeow!\n"); fprintf(stderr, "Yeow!\n"); } } if(rdst < 0) s = -s; if(rdst < 0) s = -s; vscale( qdst, s, &dst ); vscale( qdst, s, &dst ); } } /* * Decompose the rotation part of an object-to-world matrix * into Euler angles in degrees, in the given axis order. * The usual VirDir order is "yxz", with the Y-rotation closest to the world and the Z-rotation closest to the object. */ /* extract Euler angles (for application in arbitrary permutation of xyz order) from 4x4 matrix. * If T is an object-to-world matrix such that * p_obj * T = p_world * then, with axABC being a string which is some permutation of "xyz" * (so that if axABC="yxz" then A=y, B=x, C=z) then * p_obj * rotC(rc) * rotB(rb) * rotA(ra) = p_world * so if axABC="yxz" then p_obj * rotZ(rz) * rotX(rx) * rotY(ry) = p_world * with all rotations in degrees (not radians). * Given the above, then * tfm2euler( rABC, "yxz", T ) returns { ry, rx, rz } in rABC while * tfm2xyzeuler( rXYZ, "yxz", T ) returns { rx, ry, rz } in rXYZ. */ #define Tij(T,i,j) ((T).m[(i)*4+(j)]) int tfm2euler( float ABC[3], CONST char *axABC, CONST Matrix *T ) { Matrix Tn; float s = sqrt( Tij(*T,0,0)*Tij(*T,0,0) + Tij(*T,0,1)*Tij(*T,0,1) + Tij(*T,0,2)*Tij(*T,0,2) ); // estimate scaling of matrix: take first row of T as a vector int i,j; int a,b,c; int perm; float *Ta, *Tb, *Tc; float A, B, C; float Tca, cosB; if(s == 0) { ABC[0] = ABC[1] = ABC[2] = 0; return 0; } for(i=0; i<3; i++) { for(j=0; j<3; j++) Tn.m[i*4+j] = T->m[i*4+j] / s; } if(axABC == 0) { a = 1, b = 0, c = 2; // yxz } else { a = axABC[0]-'x'; b = axABC[1]-'x'; c = axABC[2]-'x'; if(((1<b) ^ (b>c) ^ (a>c) ) ? -1 : 1; Ta = &Tij(Tn, a, 0); // a'th row of T, etc. Tb = &Tij(Tn, b, 0); Tc = &Tij(Tn, c, 0); Tca = Tc[a]; // Tca * perm = sin(B) cosB = hypot( Ta[a], Tb[a] ); // hypot( Taa, Tba ) = hypot( Tcb, Tcc ) = cos(B) B = atan2( perm * Tca, cosB ); A = atan2( -perm*Tc[b], Tc[c] ); // A = atan2( -perm*Tcb, Tcc ); C = atan2( -perm*Tb[a], Ta[a] ); // C = atan2( -perm*Tba, Taa ); // Apply corrections for best accuracy if(Tca > 0.9) { // perm*sin(B) is near +1, cos(B) near zero, so Taa/Tba/Tcb/Tcc terms inaccurate. // Tca is near +1, use A+C terms, which should be quite accurate. // -perm*(Tab+Tbc) = sin(A+C) * (1 + Tca) // (Tbb-Tac) = cos(A+C) * (1 + Tca) float ApC = atan2( perm * (Ta[b] + Tb[c]), Tb[b] - Ta[c] ); // Tweak A and C so that they sum to this float delta = 0.5 * (ApC - (A + C)); if(delta > M_PI_2) delta -= M_PI; else if(delta < -M_PI_2) delta += M_PI; A += delta; C += delta; } else if(Tca < -0.9) { // perm*sin(B) is near -1, cos(B) near zero, so Taa/Tba/Tcb/Tcc terms inaccurate // Tca is near -1, use A-C terms, which should be quite accurate // perm*(Tab-Tbc) = sin(A-C) * (1 - Tca) // (Tac+Tbb) = cos(A-C) * (1 - Tca) float AmC = atan2( -perm * (Ta[b] - Tb[c]), Ta[c] + Tb[b] ); // Tweak A and C so that their difference is this. float delta = 0.5 * (AmC - (A - C)); if(delta > M_PI_2) delta -= M_PI; else if(delta < -M_PI_2) delta += M_PI; A += delta; C -= delta; } ABC[0] = A * (180/M_PI); ABC[1] = B * (180/M_PI); ABC[2] = C * (180/M_PI); return 1; } #undef Tij int tfm2xyzeuler( float rXYZ[3], CONST char *axABC, CONST Matrix *T ) { float ABC[3]; int i; int ok = tfm2euler( ABC, axABC, T ); if(!ok) return 0; if(axABC == 0) axABC = "yxz"; for(i = 0; i < 3; i++) { int ax = axABC[i] - 'x'; rXYZ[ax] = ABC[i]; } return 1; } /* /* * Decompose the rotation & translation part of a camera-to-world matrix * Decompose the rotation & translation part of a camera-to-world matrix * (which may also include some *uniform* scaling) * (which may also include some *uniform* scaling) ... ...
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!