Skip to content
GitLab
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
Hide whitespace changes
Inline Side-by-side
src/geometry.c
View file @ b92b2ae0
......@@ -363,7 +363,7 @@ float tfm2iquat( Point *iquat, CONST Matrix *T )
}
}
mag = vlength(&axis);
if(!finite(mag)) {
if(!isfinite(mag)) {
fprintf(stderr, "Yikes, tfm2quat() yields NaN?\n");
}
/* The imaginary part is a vector pointing along the axis of rotation,
......@@ -383,11 +383,12 @@ float mdet3( CONST Matrix *T )
return vdot( &v01, (CONST Point *)&T->m[8] );
}
#define Tij(i,j) T->m[(i)*4+(j)]
void tfm2quat( Quat *quat, CONST Matrix *T )
{
float ww, xx, yy, zz; /* i.e. w^2, x^2, etc. */
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;
/* A rotation matrix is
......@@ -397,17 +398,15 @@ void tfm2quat( Quat *quat, CONST Matrix *T )
* and
* ww+xx+yy+zz = ss
*/
if(s > 0) {
/* OK */
} else {
if(s == 0) {
quat->q[0] = 1;
quat->q[1] = quat->q[2] = quat->q[3] = 0;
return;
}
ww = (s + T->m[0*4+0] + T->m[1*4+1] + T->m[2*4+2]); /* 4 * w^2 */
xx = (s + T->m[0*4+0] - T->m[1*4+1] - T->m[2*4+2]);
yy = (s - T->m[0*4+0] + T->m[1*4+1] - T->m[2*4+2]);
zz = (s - T->m[0*4+0] - T->m[1*4+1] + T->m[2*4+2]);
ww = (s + Tij(0,0) + Tij(1,1) + Tij(2,2)); /* 4 * w^2 */
xx = (s + Tij(0,0) - Tij(1,1) - Tij(2,2));
yy = (s - Tij(0,0) + Tij(1,1) - Tij(2,2));
zz = (s - Tij(0,0) - Tij(1,1) + Tij(2,2));
max = ww;
if(max < xx) max = xx, best = 1;
......@@ -416,45 +415,46 @@ void tfm2quat( Quat *quat, CONST Matrix *T )
switch(best) {
case 0: /* ww == max */
w = sqrtf(ww) * 2; /* 4w */
x = (T->m[2*4+1] - T->m[1*4+2]) / w; /* 4wx/4w */
y = (T->m[0*4+2] - T->m[2*4+0]) / w; /* 4wy/4w */
z = (T->m[1*4+0] - T->m[0*4+1]) / w; /* 4wz/4w */
w = sqrt(ww) * 2; /* 4w */
x = (Tij(2,1) - Tij(1,2)) / w; /* 4wx/4w */
y = (Tij(0,2) - Tij(2,0)) / w; /* 4wy/4w */
z = (Tij(1,0) - Tij(0,1)) / w; /* 4wz/4w */
w *= .25f; /* w */
break;
case 1: /* xx == max */
x = sqrtf(xx) * 2; /* 4x */
w = (T->m[2*4+1] - T->m[1*4+2]) / x; /* 4wx/4x */
y = (T->m[1*4+0] + T->m[0*4+1]) / x; /* 4xy/4x */
z = (T->m[0*4+2] + T->m[2*4+0]) / x; /* 4xz/4x */
x = sqrt(xx) * 2; /* 4x */
w = (Tij(2,1) - Tij(1,2)) / x; /* 4wx/4x */
y = (Tij(1,0) + Tij(0,1)) / x; /* 4xy/4x */
z = (Tij(0,2) + Tij(2,0)) / x; /* 4xz/4x */
x *= .25f; /* x */
break;
case 2: /* yy == max */
y = sqrtf(yy) * 2; /* 4y */
w = (T->m[0*4+2] - T->m[2*4+0]) / y; /* 4wy/4y */
x = (T->m[1*4+0] + T->m[0*4+1]) / y; /* 4xy/4y */
z = (T->m[2*4+1] + T->m[1*4+2]) / y; /* 4yz/4y */
y = sqrt(yy) * 2; /* 4y */
w = (Tij(0,2) - Tij(2,0)) / y; /* 4wy/4y */
x = (Tij(1,0) + Tij(0,1)) / y; /* 4xy/4y */
z = (Tij(2,1) + Tij(1,2)) / y; /* 4yz/4y */
y *= .25f; /* y */
break;
default: /* zz == max */
z = sqrtf(zz) * 2; /* 4z */
w = (T->m[1*4+0] - T->m[0*4+1]) / z; /* 4wz/4z */
x = (T->m[0*4+2] + T->m[2*4+0]) / z; /* 4xz/4z */
y = (T->m[2*4+1] + T->m[1*4+2]) / z; /* 4yz/4z */
z = sqrt(zz) * 2; /* 4z */
w = (Tij(1,0) - Tij(0,1)) / z; /* 4wz/4z */
x = (Tij(0,2) + Tij(2,0)) / z; /* 4xz/4z */
y = (Tij(2,1) + Tij(1,2)) / z; /* 4yz/4z */
z *= .25f; /* z */
break;
}
s = sqrtf(s);
s = sqrt(s);
quat->q[0] = -w/s;
quat->q[1] = x/s;
quat->q[2] = y/s;
quat->q[3] = z/s;
}
#undef Tij
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 )
rdst = (1-frac)*rfrom + frac*rto;
vlerp( &dst, frac, qfrom, &tto );
s = 1/sqrtf(rdst*rdst + VDOT(&dst,&dst));
if(!finite(s)) {
if(!isfinite(s)) {
fprintf(stderr, "Yeow!\n");
}
if(rdst < 0) s = -s;
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
* (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

Contact help+service@ncsa.illinois.edu with questions regarding this site. | All rights reserved. ©2022 Board of Trustees of the University of Illinois. | Web Privacy Notice