Commit b92b2ae0 authored by slevy's avatar slevy
Browse files

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<<a) | (1<<b) | (1<<c)) != ((1<<0)|(1<<1)|(1<<2))) {
fprintf(stderr, "tfm2euler(): axis order must be a permutation of \"xyz\" (e.g. \"yxz\"), not \"%s\"\n", axABC);
ABC[0] = ABC[1] = ABC[2] = 0;
return 0;
}
}
perm = ( (a>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!
Please register or to comment