diff options
Diffstat (limited to 'src/poser_daveortho.c')
-rw-r--r-- | src/poser_daveortho.c | 117 |
1 files changed, 107 insertions, 10 deletions
diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c index e81e154..9d02bb3 100644 --- a/src/poser_daveortho.c +++ b/src/poser_daveortho.c @@ -265,7 +265,8 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); // FLT ydist1 = 1.0 / uhat_len; //0.25*PI / uhat_len; // FLT ydist2 = 1.0 / rhat_len; //0.25*PI / rhat_len; FLT ydist = 1.0 / urhat_len; - //printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, ydist); + printf("ydist %f\n", ydist); +// printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, ydist); //-------------------- // Rescale the axies to be of the proper length @@ -282,7 +283,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); if( x_y != x_y ) x_y = 0; if( y_y != y_y ) y_y = 0; if( z_y != z_y ) z_y = 0; -/* + // Exhaustively flip the minus sign of the z axis until we find the right one . . . FLT bestErr = 9999.0; FLT xy_dot2 = x[0][0]*y[0][0] + x[2][0]*y[2][0]; @@ -302,7 +303,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); FLT cx,cy,cz; CrossProduct(cx,cy,cz,x[0][0],x_y,x[2][0],y[0][0],y_y,y[2][0]); FLT hand = cx*z[0][0] + cy*z_y + cz*z[2][0]; - printf("err %f hand %f\n", err, hand); +// printf("err %f hand %f\n", err, hand); // If we are the best right-handed frame so far //if (hand > 0 && err < bestErr) { x[1][0]=x_y; y[1][0]=y_y; z[1][0]=z_y; bestErr=err; } @@ -313,9 +314,9 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); } x_y = -x_y; } - printf("bestErr %f\n", bestErr); -*/ +// printf("bestErr %f\n", bestErr); +/* //------------------------- // A test version of the rescaling to the proper length //------------------------- @@ -338,7 +339,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); if( y_y != y_y ) y_y = 0; if( z_y != z_y ) z_y = 0; - printf( "---> %f %f %f\n", x_y, y_y, z_y ); +// printf( "---> %f %f %f\n", x_y, y_y, z_y ); // Exhaustively flip the minus sign of the z axis until we find the right one . . . FLT bestErr = 9999.0; @@ -359,7 +360,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); FLT cx,cy,cz; CrossProduct(cx,cy,cz,x2[0][0],x_y,x2[2][0],y2[0][0],y_y,y2[2][0]); FLT hand = cx*z2[0][0] + cy*z_y + cz*z2[2][0]; - printf("err %f hand %f\n", err, hand); + //printf("err %f hand %f\n", err, hand); // If we are the best right-handed frame so far if (hand > 0 && err < bestErr) { x2[1][0]=x_y; y2[1][0]=y_y; z2[1][0]=z_y; bestErr=err; } @@ -380,7 +381,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); } } ydist = bestYdist; - +*/ /* for (i=0; i<nPoints; i++) { FLT x1 = x[0][0]*X[0][i] + y[0][0]*X[1][i] + z[0][0]*X[2][i]; @@ -441,7 +442,103 @@ PRINT(ab,2,1); T[2][0]=R[2][0]; T[2][1]=R[2][1]; T[2][2]=R[2][2]; T[2][3]=trans[2]; T[3][0]=0.0; T[3][1]=0.0; T[3][2]=0.0; T[3][3]=1.0; - PRINT_MAT(T,4,4); + + FLT T2[4][4]; + + //------------------- + // Orthogonalize the matrix + //------------------- + FLT temp[4][4]; + FLT quat[4], quatNorm[4]; + FLT euler[3]; + + + //------------------- + // Orthogonalize the matrix + //------------------- + PRINT_MAT(T,4,4); + +#if 1 +// matrix44transpose(T2, T); //Transpose so we are + matrix44copy((FLT*)T2,(FLT*)T); + cross3d( &T2[1][0], &T2[0][0], &T2[2][0] ); + cross3d( &T2[2][0], &T2[1][0], &T2[0][0] ); //Replace axes in-place. + matrix44copy((FLT*)T,(FLT*)T2); +// matrix44transpose(T, T2); + +#endif + + normalize3d( &T[0][0], &T[0][0] ); + normalize3d( &T[1][0], &T[1][0] ); + normalize3d( &T[2][0], &T[2][0] ); + //Change handedness + + T[1][0]*=-1; + T[1][1]*=-1; + T[1][2]*=-1; + +/* + //Check Orthogonality. Yep. It's orthogonal. + FLT tmp[3]; + cross3d( tmp, &T[0][0], &T[1][0] ); + printf( "M3: %f\n", magnitude3d( tmp ) ); + cross3d( tmp, &T[2][0], &T[1][0] ); + printf( "M3: %f\n", magnitude3d( tmp ) ); + cross3d( tmp, &T[2][0], &T[0][0] ); + printf( "M3: %f\n", magnitude3d( tmp ) ); +*/ + +// PRINT_MAT(T,4,4); + +#if 1 + + //matrix44copy(T2,T); + matrix44transpose((FLT*)T2,(FLT*)T); + + quatfrommatrix( quat, &T2[0][0] ); + printf( "QM: %f\n", quatmagnitude( quat ) ); + quatnormalize(quatNorm,quat); + quattoeuler(euler,quatNorm); + quattomatrix( &T2[0][0], quatNorm ); + + PRINT_MAT(T2,4,4); + printf("rot %f %f %f len %f\n", euler[0], euler[1], euler[2], quatmagnitude(quat)); +// PRINT(T,4,4); + +// matrix44copy(temp,T2); + matrix44transpose((FLT*)temp,(FLT*)T2); + + +// matrix44transpose(T2, temp); +// memcpy(T2,temp,16*sizeof(float)); + for (i=0; i<3; i++) { + for (j=0; j<3; j++) { + T[i][j] = temp[i][j]; + } + } + +/* PRINT(T2,4,4); */ +#endif + + T[1][0]*=-1; + T[1][1]*=-1; + T[1][2]*=-1; + + +/* + CrossProduct(T[0][2],T[1][2],T[2][2], T[0][0],T[1][0],T[2][0], T[0][1],T[1][1],T[2][1]); + CrossProduct(T[0][0],T[1][0],T[2][0], T[0][1],T[1][1],T[2][1], T[0][2],T[1][2],T[2][2]); + CrossProduct(T[0][1],T[1][1],T[2][1], T[0][2],T[1][2],T[2][2], T[0][0],T[1][0],T[2][0]); + float xlen = sqrt(T[0][0]*T[0][0] + T[1][0]*T[1][0] + T[2][0]*T[2][0]); + float ylen = sqrt(T[0][1]*T[0][1] + T[1][1]*T[1][1] + T[2][1]*T[2][1]); + float zlen = sqrt(T[0][2]*T[0][2] + T[1][0]*T[1][2] + T[2][2]*T[2][2]); + T[0][0]/=xlen; T[1][0]/=xlen; T[2][0]/=xlen; + T[0][1]/=ylen; T[1][1]/=ylen; T[2][1]/=ylen; + T[0][2]/=zlen; T[1][2]/=zlen; T[2][2]/=zlen; +*/ + + + // PRINT_MAT(T,4,4); //------------------- // Plot the output points //------------------- @@ -453,7 +550,7 @@ PRINT(ab,2,1); S_out[1][i] = atan2(Tz, Ty); // vert //S_out[0][i] = Tx; //S_out[1][i] = Tz; - printf("point %i Txyz %f %f %f in %f %f out %f %f morph %f %f\n", i, Tx,Ty,Tz, S_in[0][i], S_in[1][i], S_out[0][i], S_out[1][i], S_morph[0][i], S_morph[1][i]); +// printf("point %i Txyz %f %f %f in %f %f out %f %f morph %f %f\n", i, Tx,Ty,Tz, S_in[0][i], S_in[1][i], S_out[0][i], S_out[1][i], S_morph[0][i], S_morph[1][i]); } } |