/* table.c (C) 1997 Mark A. Livingston */ /* Any non-commercial use of this code is permitted so long as */ /* the above copyright line appears in the file. Please contact */ /* the author regarding permission for commercial use. */ #include #include #include #include #include "quatext.h" #include "table.h" #ifdef USE_GAUSS #include "config.h" #include "data.h" #endif #define TABLENODEFMT "{ %lf, %lf, %lf }, { %lf, %lf, %lf, %lf }" #define NEWXYZQUAT(n) (q_xyz_quat_type *) malloc((n)*sizeof( q_xyz_quat_type )) #define NEWVEC(n) ( q_vec_type * ) malloc( (n) * sizeof( q_vec_type ) ) #define INDEX(i,j,k,x,y,z) ( ( ( (i) * (y) + (j) ) * (z) ) + (k) ) static q_vec_type Xaxis = { 1.0, 0.0, 0.0 }; static q_vec_type Yaxis = { 0.0, 1.0, 0.0 }; static q_vec_type Zaxis = { 0.0, 0.0, 1.0 }; static int xSteps, ySteps, zSteps; static int xSlice, ySlice, zSlice; static q_vec_type minCell, maxCell, cellIncr; static q_xyz_quat_type *table; /* ** Reads the correction table from the named file. Assumes the format is: ** first line: "xSize ySize zSize" ( number of points in each dim ) ** second line: "xMin yMin zMin" ( minimum coordinate in each dim ) ** third line: "xMax yMax zMax" ( minimum coordinate in each dim ) ** remaining lines: "{ x, y, z }, { x, y, z, w }" ** where the first line is three integers, the second and third are 3 doubles ** and the other lines are 7 doubles. Assumes that the file is in row major ** form (z rotates fastest, then y, then x slowest). */ int tabReadTable( char *fname, q_vec_type minRetVal, q_vec_type maxRetVal ) { char buf[256]; int i, tableCells; FILE *fp; q_xyz_quat_type *t; if( ( fp = fopen( fname, "r" ) ) == NULL ) { fprintf( stderr, "Unable to open table file '%s'\n", fname ); return -1; } /* Read table header */ if( fgets( buf, 255, fp ) == NULL ) { fprintf( stderr, "Error reading table header (sizes)\n" ); fclose( fp ); return -1; } sscanf( buf, "%d %d %d", &xSteps, &ySteps, &zSteps ); if( fgets( buf, 255, fp ) == NULL ) { fprintf( stderr, "Error reading table header (minima)\n" ); fclose( fp ); return -1; } sscanf( buf, "%lf %lf %lf", &minCell[Q_X], &minCell[Q_Y], &minCell[Q_Z] ); q_vec_copy( minRetVal, minCell ); if( fgets( buf, 255, fp ) == NULL ) { fprintf( stderr, "Error reading table header (maxima)\n" ); fclose( fp ); return -1; } sscanf( buf, "%lf %lf %lf", &maxCell[Q_X], &maxCell[Q_Y], &maxCell[Q_Z] ); q_vec_copy( maxRetVal, maxCell ); /* Compute dimensions of a cell */ q_vec_subtract( cellIncr, maxCell, minCell ); if( xSteps > 1 ) cellIncr[Q_X] /= (double) ( xSteps - 1 ); else cellIncr[Q_X] = 0.0; if( ySteps > 1 ) cellIncr[Q_Y] /= (double) ( ySteps - 1 ); else cellIncr[Q_Y] = 0.0; if( zSteps > 1 ) cellIncr[Q_Z] /= (double) ( zSteps - 1 ); else cellIncr[Q_Z] = 0.0; /* Compute steps in each dimension */ xSlice = ySteps * zSteps; ySlice = zSteps; zSlice = 1; /* Allocate table */ tableCells = xSteps * ySteps * zSteps; table = NEWXYZQUAT( tableCells ); if( table == NULL ) { fprintf( stderr, "Unable to allocate memory for table\n" ); fclose( fp ); return -1; } /* read table */ for( t = table, i = 0; i < tableCells; i++, t++ ) { if( fgets( buf, 255, fp ) == NULL ) { fprintf( stderr, "Error reading cell %d from file\n", i ); fclose( fp ); free( table ); return -1; } sscanf( buf, TABLENODEFMT, &t->xyz[Q_X], &t->xyz[Q_Y], &t->xyz[Q_Z], &t->quat[Q_X], &t->quat[Q_Y], &t->quat[Q_Z], &t->quat[Q_W] ); } /* end for(t,i) */ fclose( fp ); return 0; } /* ** Finds cell in table, then tri-lerps the position and tri-slerps the ** orientation correction, and applies it to the reading. */ int tabCorrectReading( q_xyz_quat_type *asceFsens ) { int iX, iY, iZ, code, retcode; register int idx; double x, y, z; double fx, fy, fz, mx, my, mz, fyfz, mymz, myfz, fymz; double fxfz, fxmz, mxfz, mxmz, fxfy, fxmy, mxfy, mxmy; q_type UxUy, UxLy, LxUy, LxLy, Ux, Lx, srodFsens, sensFsrod; q_type Ly, Uy; double w[8]; q_xyz_quat_type q_to_avg[8], correction, tmp; retcode = code = OUTCODE( asceFsens->xyz, minCell, maxCell ); if( code == CTR ) { /* Find cell */ x = ( asceFsens->xyz[Q_X] - minCell[Q_X] ) / cellIncr[Q_X]; y = ( asceFsens->xyz[Q_Y] - minCell[Q_Y] ) / cellIncr[Q_Y]; z = ( asceFsens->xyz[Q_Z] - minCell[Q_Z] ) / cellIncr[Q_Z]; iX = (int) x; fx = x - (double) iX; mx = 1.0 - fx; iY = (int) y; fy = y - (double) iY; my = 1.0 - fy; iZ = (int) z; fz = z - (double) iZ; mz = 1.0 - fz; /* ** The reading is in the cell bounded above by table[iX][iY][iZ] and ** below by table[iX+1][iY+1][iZ+1] and the weights for each endpoint ** in the respective dimensions are in {fm}{xyz}. */ /* faster rvalues */ fyfz = fy * fz; mymz = my * mz; myfz = my * fz; fymz = fy * mz; idx = iX * xSlice + iY * ySlice + iZ; /* zSlice = 1 */ #ifdef USE_SVD #ifdef USE_GAUSS /* use Gaussian-weighted averaging, SVD for the quats */ q_vec_copy( q_to_avg[0].xyz, table[ idx ].xyz ); q_copy( q_to_avg[0].quat, table[ idx ].quat ); w[0] = KernelValue( 0.05, q_vec_distance( asceFsens->xyz, table[ idx ].xyz ) ); q_vec_copy( q_to_avg[1].xyz, table[ ++idx ].xyz ); q_copy( q_to_avg[1].quat, table[ idx ].quat ); w[1] = KernelValue( 0.05, q_vec_distance( asceFsens->xyz, table[ idx ].xyz ) ); q_vec_copy( q_to_avg[2].xyz, table[ idx += ySlice ].xyz ); q_copy( q_to_avg[2].quat, table[ idx ].quat ); w[2] = KernelValue( 0.05, q_vec_distance( asceFsens->xyz, table[ idx ].xyz ) ); q_vec_copy( q_to_avg[3].xyz, table[ --idx ].xyz ); q_copy( q_to_avg[3].quat, table[ idx ].quat ); w[3] = KernelValue( 0.05, q_vec_distance( asceFsens->xyz, table[ idx ].xyz ) ); q_vec_copy( q_to_avg[4].xyz, table[ idx += xSlice ].xyz ); q_copy( q_to_avg[4].quat, table[ idx ].quat ); w[4] = KernelValue( 0.05, q_vec_distance( asceFsens->xyz, table[ idx ].xyz ) ); q_vec_copy( q_to_avg[5].xyz, table[ ++idx ].xyz ); q_copy( q_to_avg[5].quat, table[ idx ].quat ); w[5] = KernelValue( 0.05, q_vec_distance( asceFsens->xyz, table[ idx ].xyz ) ); q_vec_copy( q_to_avg[6].xyz, table[ idx -= ySlice ].xyz ); q_copy( q_to_avg[6].quat, table[ idx ].quat ); w[6] = KernelValue( 0.05, q_vec_distance( asceFsens->xyz, table[ idx ].xyz ) ); q_vec_copy( q_to_avg[7].xyz, table[ --idx ].xyz ); q_copy( q_to_avg[7].quat, table[ idx ].quat ); w[7] = KernelValue( 0.05, q_vec_distance( asceFsens->xyz, table[ idx ].xyz ) ); qx_xyz_quat_wgt_avg( &correction, q_to_avg, w, 8 ); #if 1 /* q_vec_invert( correction.xyz, correction.xyz ); */ /* q_xform( correction.xyz, correction.quat, correction.xyz ); */ q_vec_add( asceFsens->xyz, asceFsens->xyz, correction.xyz ); #if 1 q_invert( sensFsrod, correction.quat ); q_mult( asceFsens->quat, asceFsens->quat, sensFsrod ); q_normalize( asceFsens->quat, asceFsens->quat ); #endif #else /* q_vec_invert( correction.xyz, correction.xyz ); */ q_invert( correction.quat, correction.quat ); q_xyz_quat_compose( &tmp, asceFsens, &correction ); q_vec_copy( asceFsens->xyz, tmp.xyz ); q_copy( asceFsens->quat, tmp.quat ); #endif #else /* use linear-weighted averaging, SVD for the quats */ q_vec_copy( q_to_avg[0].xyz, table[ idx ].xyz ); q_copy( q_to_avg[0].quat, table[ idx ].quat ); w[0] = mymz * mx; q_vec_copy( q_to_avg[1].xyz, table[ ++idx ].xyz ); q_copy( q_to_avg[1].quat, table[ idx ].quat ); w[1] = myfz * mx; q_vec_copy( q_to_avg[2].xyz, table[ idx += ySlice ].xyz ); q_copy( q_to_avg[2].quat, table[ idx ].quat ); w[2] = fyfz * mx; q_vec_copy( q_to_avg[3].xyz, table[ --idx ].xyz ); q_copy( q_to_avg[3].quat, table[ idx ].quat ); w[3] = fymz * mx; q_vec_copy( q_to_avg[4].xyz, table[ idx += xSlice ].xyz ); q_copy( q_to_avg[4].quat, table[ idx ].quat ); w[4] = fymz * fx; q_vec_copy( q_to_avg[5].xyz, table[ ++idx ].xyz ); q_copy( q_to_avg[5].quat, table[ idx ].quat ); w[5] = fyfz * fx; q_vec_copy( q_to_avg[6].xyz, table[ idx -= ySlice ].xyz ); q_copy( q_to_avg[6].quat, table[ idx ].quat ); w[6] = myfz * fx; q_vec_copy( q_to_avg[7].xyz, table[ --idx ].xyz ); q_copy( q_to_avg[7].quat, table[ idx ].quat ); w[7] = mymz * fx; qx_xyz_quat_wgt_avg( &correction, q_to_avg, w, 8 ); #if 1 /* q_vec_invert( correction.xyz, correction.xyz ); */ /* q_xform( correction.xyz, correction.quat, correction.xyz ); */ q_vec_add( asceFsens->xyz, asceFsens->xyz, correction.xyz ); #if 1 q_invert( sensFsrod, correction.quat ); q_mult( asceFsens->quat, asceFsens->quat, sensFsrod ); q_normalize( asceFsens->quat, asceFsens->quat ); #endif #else /* q_vec_invert( correction.xyz, correction.xyz ); */ q_invert( correction.quat, correction.quat ); q_xyz_quat_compose( &tmp, asceFsens, &correction ); q_vec_copy( asceFsens->xyz, tmp.xyz ); q_copy( asceFsens->quat, tmp.quat ); #endif #endif /* USE_GAUSS */ #else /* trilerp position and tri-slerp orientation */ asceFsens->xyz[Q_X] += mymz * mx * table[ idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += mymz * mx * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += mymz * mx * table[ idx ].xyz[Q_Z]; asceFsens->xyz[Q_X] += myfz * mx * table[ ++idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += myfz * mx * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += myfz * mx * table[ idx ].xyz[Q_Z]; asceFsens->xyz[Q_X] += fyfz * mx * table[ idx += ySlice ].xyz[Q_X]; asceFsens->xyz[Q_Y] += fyfz * mx * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += fyfz * mx * table[ idx ].xyz[Q_Z]; asceFsens->xyz[Q_X] += fymz * mx * table[ --idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += fymz * mx * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += fymz * mx * table[ idx ].xyz[Q_Z]; asceFsens->xyz[Q_X] += fymz * fx * table[ idx += xSlice ].xyz[Q_X]; asceFsens->xyz[Q_Y] += fymz * fx * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += fymz * fx * table[ idx ].xyz[Q_Z]; asceFsens->xyz[Q_X] += fyfz * fx * table[ ++idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += fyfz * fx * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += fyfz * fx * table[ idx ].xyz[Q_Z]; asceFsens->xyz[Q_X] += myfz * fx * table[ idx -= ySlice ].xyz[Q_X]; asceFsens->xyz[Q_Y] += myfz * fx * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += myfz * fx * table[ idx ].xyz[Q_Z]; asceFsens->xyz[Q_X] += mymz * fx * table[ --idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += mymz * fx * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += mymz * fx * table[ idx ].xyz[Q_Z]; /* trislerp orientation */ idx = iX * xSlice + iY * ySlice + iZ; q_slerp( LxLy, table[ idx ].quat, table[ idx + 1 ].quat, fz ); idx += ySlice; q_slerp( LxUy, table[ idx ].quat, table[ idx + 1 ].quat, fz ); idx += xSlice; q_slerp( UxUy, table[ idx ].quat, table[ idx + 1 ].quat, fz ); idx -= ySlice; q_slerp( UxLy, table[ idx ].quat, table[ idx + 1 ].quat, fz ); q_slerp( Ux, UxLy, UxUy, fy ); q_slerp( Lx, LxLy, LxUy, fy ); q_slerp( srodFsens, Lx, Ux, fx ); q_invert( sensFsrod, srodFsens ); q_mult( asceFsens->quat, asceFsens->quat, sensFsrod ); q_normalize( asceFsens->quat, asceFsens->quat ); #endif /* USE_SVD */ } else { /* we are outside the table -- want to extrapolate */ if( code & MIN_X ) { code &= ~MIN_X; if( code == CTR ) { /* we were only outside the -X */ /* Find the square along the -X face to use */ y = ( asceFsens->xyz[Q_Y] - minCell[Q_Y] ) / cellIncr[Q_Y]; z = ( asceFsens->xyz[Q_Z] - minCell[Q_Z] ) / cellIncr[Q_Z]; iY = (int) y; fy = y - (double) iY; my = 1.0 - fy; iZ = (int) z; fz = z - (double) iZ; mz = 1.0 - fz; /* ** The reading projects to the rectangle along the -X face of the ** table bounded by table[0][iY][iZ] and table[0][iY+1][iZ+1]. */ /* faster rvalues */ fyfz = fy * fz; fymz = fy * mz; myfz = my * fz; mymz = my * mz; idx = iY * ySlice + iZ; /* bilerp position */ asceFsens->xyz[Q_X] += mymz * table[ idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += mymz * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += mymz * table[ idx ].xyz[Q_Z]; asceFsens->xyz[Q_X] += myfz * table[ ++idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += myfz * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += myfz * table[ idx ].xyz[Q_Z]; asceFsens->xyz[Q_X] += fyfz * table[ idx += ySlice ].xyz[Q_X]; asceFsens->xyz[Q_Y] += fyfz * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += fyfz * table[ idx ].xyz[Q_Z]; asceFsens->xyz[Q_X] += fymz * table[ --idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += fymz * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += fymz * table[ idx ].xyz[Q_Z]; /* bislerp orientation */ idx = iY * ySlice + iZ; q_slerp( Ly, table[ idx ].quat, table[ idx + 1 ].quat, fz ); idx += ySlice; q_slerp( Uy, table[ idx ].quat, table[ idx + 1 ].quat, fz ); q_slerp( srodFsens, Ly, Uy, fy ); q_invert( sensFsrod, srodFsens ); q_mult( asceFsens->quat, asceFsens->quat, sensFsrod ); q_normalize( asceFsens->quat, asceFsens->quat ); } else { /* outside -X and something else */ if( code & MIN_Y ) { code &= ~MIN_Y; switch( code ) { case CTR: /* outside -X and -Y, center in Z */ z = ( asceFsens->xyz[Q_Z] - minCell[Q_Z] ) / cellIncr[Q_Z]; iZ = (int) z; fz = z - (double) iZ; mz = 1.0 - fz; idx = iZ; /* ** The reading projects to an edge of the table, bounded by ** table[0][0][iZ] and table[0][0][iZ+1]. */ /* lerp position */ asceFsens->xyz[Q_X] += mz * table[ idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += mz * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += mz * table[ idx ].xyz[Q_Z]; asceFsens->xyz[Q_X] += fz * table[ ++idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += fz * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += fz * table[ idx ].xyz[Q_Z]; /* slerp orientation */ q_slerp( srodFsens, table[ iZ ].quat, table[ iZ + 1 ].quat, fz ); q_invert( sensFsrod, srodFsens ); q_mult( asceFsens->quat, asceFsens->quat, sensFsrod ); q_normalize( asceFsens->quat, asceFsens->quat ); break; case MIN_Z: /* outside -X, -Y, and -Z */ /* apply correction at corner */ asceFsens->xyz[Q_X] += table[ 0 ].xyz[Q_X]; asceFsens->xyz[Q_Y] += table[ 0 ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += table[ 0 ].xyz[Q_Z]; q_invert( sensFsrod, table[ 0 ].quat ); q_mult( asceFsens->quat, asceFsens->quat, sensFsrod ); q_normalize( asceFsens->quat, asceFsens->quat ); break; case MAX_Z: /* outside -X, -Y, and +Z */ /* apply correction at corner */ asceFsens->xyz[Q_X] += table[ zSteps - 1 ].xyz[Q_X]; asceFsens->xyz[Q_Y] += table[ zSteps - 1 ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += table[ zSteps - 1 ].xyz[Q_Z]; q_invert( sensFsrod, table[ zSteps - 1 ].quat ); q_mult( asceFsens->quat, asceFsens->quat, sensFsrod ); q_normalize( asceFsens->quat, asceFsens->quat ); break; } /* end switch( possible Z locations ) */ } else if( code & MAX_Y ) { code &= ~MAX_Y; switch( code ) { case CTR: /* outside -X, +Y, center in Z */ z = ( asceFsens->xyz[Q_Z] - minCell[Q_Z] ) / cellIncr[Q_Z]; iZ = (int) z; fz = z - (double) iZ; mz = 1.0 - fz; idx = ( ySteps - 1 ) * ySlice + iZ; /* ** The reading projects to an edge of the table, bounded by ** table[0][ySteps-1][iZ] and table[0][ySteps-1][iZ+1]. */ /* lerp position */ asceFsens->xyz[Q_X] += mz * table[ idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += mz * table[ idx ].xyz[Q_X]; asceFsens->xyz[Q_Z] += mz * table[ idx ].xyz[Q_X]; asceFsens->xyz[Q_X] += fz * table[ ++idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += fz * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += fz * table[ idx ].xyz[Q_Z]; /* slerp orientation */ idx = ( ySteps - 1 ) * ySlice + iZ; q_slerp( srodFsens, table[ idx ].quat, table[ idx+1 ].quat, fz ); q_invert( sensFsrod, srodFsens ); q_mult( asceFsens->quat, asceFsens->quat, sensFsrod ); q_normalize( asceFsens->quat, asceFsens->quat ); break; case MIN_Z: /* outside -X, +Y, and -Z */ /* apply correction at corner */ idx = ( ySteps - 1 ) * ySlice; asceFsens->xyz[Q_X] += table[ idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += table[ idx ].xyz[Q_Z]; q_invert( sensFsrod, table[ idx ].quat ); q_mult( asceFsens->quat, asceFsens->quat, sensFsrod ); q_normalize( asceFsens->quat, asceFsens->quat ); break; case MAX_Z: /* outside -X, +Y, and +Z */ /* apply correction at corner */ idx = ( ySteps - 1 ) * ySlice + ( zSteps - 1 ); asceFsens->xyz[Q_X] += table[ idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += table[ idx ].xyz[Q_Z]; q_invert( sensFsrod, table[ idx ].quat ); q_mult( asceFsens->quat, asceFsens->quat, sensFsrod ); q_normalize( asceFsens->quat, asceFsens->quat ); break; } /* end switch of possible Z locations */ } else if( code & MIN_Z ) { /* ** At this point, we know that we are outside -X and -Z but ** inside in Y. So we just lerp along that edge. */ y = ( asceFsens->xyz[Q_Y] - minCell[Q_Y] ) / cellIncr[Q_Y]; iY = (int) y; fy = y - (double) iY; my = 1.0 - fy; idx = iY * ySlice; /* lerp position */ asceFsens->xyz[Q_X] += my * table[ idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += my * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += my * table[ idx ].xyz[Q_Z]; asceFsens->xyz[Q_X] += fy * table[ idx += ySlice ].xyz[Q_X]; asceFsens->xyz[Q_Y] += fy * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += fy * table[ idx ].xyz[Q_Z]; /* slerp orientation */ idx = iY * ySlice; q_slerp( srodFsens, table[idx].quat, table[idx+ySlice].quat, fy ); q_invert( sensFsrod, srodFsens ); q_mult( asceFsens->quat, asceFsens->quat, sensFsrod ); q_normalize( asceFsens->quat, asceFsens->quat ); } else if( code & MAX_Z ) { /* had better be true by now */ /* Outside -X and +Z, similarly to previous case */ y = ( asceFsens->xyz[Q_Y] - minCell[Q_Y] ) / cellIncr[Q_Y]; iY = (int) y; fy = y - (double) iY; my = 1.0 - fy; idx = iY * ySlice + ( zSteps - 1 ); /* lerp position */ asceFsens->xyz[Q_X] += my * table[ idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += my * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += my * table[ idx ].xyz[Q_Z]; asceFsens->xyz[Q_X] += fy * table[ idx += ySlice ].xyz[Q_X]; asceFsens->xyz[Q_Y] += fy * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += fy * table[ idx ].xyz[Q_Z]; /* slerp orientation */ idx = iY * ySlice + ( zSteps - 1 ); q_slerp( srodFsens, table[idx].quat, table[idx+ySlice].quat, fy ); q_invert( sensFsrod, srodFsens ); q_mult( asceFsens->quat, asceFsens->quat, sensFsrod ); q_normalize( asceFsens->quat, asceFsens->quat ); } /* end cases for outside -X and something else */ } /* end outside -X and something else */ } else if( code & MAX_X ) { code &= ~MAX_X; if( code == CTR ) { /* we were outside only the +X */ /* Find the square along the -X face to use */ y = ( asceFsens->xyz[Q_Y] - minCell[Q_Y] ) / cellIncr[Q_Y]; z = ( asceFsens->xyz[Q_Z] - minCell[Q_Z] ) / cellIncr[Q_Z]; iY = (int) y; fy = y - (double) iY; my = 1.0 - fy; iZ = (int) z; fz = z - (double) iZ; mz = 1.0 - fz; /* ** The reading projects to the rectangle along the +X face, bounded ** by table[xSteps-1][iY][iZ] and table[xSteps-1][iY+1][iZ+1]. */ /* faster rvalues */ fyfz = fy * fz; fymz = fy * mz; myfz = my * fz; mymz = my * mz; idx = ( xSteps - 1 ) * xSlice + iY * ySlice + iZ; /* bilerp position */ asceFsens->xyz[Q_X] += mymz * table[ idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += mymz * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += mymz * table[ idx ].xyz[Q_Z]; asceFsens->xyz[Q_X] += myfz * table[ ++idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += myfz * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += myfz * table[ idx ].xyz[Q_Z]; asceFsens->xyz[Q_X] += fyfz * table[ idx += ySlice ].xyz[Q_X]; asceFsens->xyz[Q_Y] += fyfz * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += fyfz * table[ idx ].xyz[Q_Z]; asceFsens->xyz[Q_X] += fymz * table[ --idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += fymz * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += fymz * table[ idx ].xyz[Q_Z]; /* bislerp orientation */ idx = ( xSteps - 1 ) * xSlice + iY * ySlice + iZ; q_slerp( Ly, table[ idx ].quat, table[ idx + 1 ].quat, fz ); idx += ySlice; q_slerp( Uy, table[ idx ].quat, table[ idx + 1 ].quat, fz ); q_slerp( srodFsens, Ly, Uy, fy ); q_invert( sensFsrod, srodFsens ); q_mult( asceFsens->quat, asceFsens->quat, sensFsrod ); q_normalize( asceFsens->quat, asceFsens->quat ); } else { /* outside +X and something else */ if( code & MIN_Y ) { code &= ~MIN_Y; switch( code ) { case CTR: /* outside +X and -Y, center in Z */ z = ( asceFsens->xyz[Q_Z] - minCell[Q_Z] ) / cellIncr[Q_Z]; iZ = (int) z; fz = z - (double) iZ; mz = 1.0 - fz; idx = ( xSteps - 1 ) * xSlice + iZ; /* ** The reading projects to an edge of the table, bounded by ** table[xSteps-1][0][iZ] and table[xSteps-1][0][iZ+1]. */ /* lerp position */ asceFsens->xyz[Q_X] += mz * table[ idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += mz * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += mz * table[ idx ].xyz[Q_Z]; asceFsens->xyz[Q_X] += fz * table[ ++idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += fz * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += fz * table[ idx ].xyz[Q_Z]; /* slerp orientation */ idx = ( xSteps - 1 ) * xSlice + iZ; q_slerp( srodFsens, table[ idx ].quat, table[ idx+1 ].quat, fz ); q_invert( sensFsrod, srodFsens ); q_mult( asceFsens->quat, asceFsens->quat, sensFsrod ); q_normalize( asceFsens->quat, asceFsens->quat ); break; case MIN_Z: /* outside +X, -Y, and -Z */ /* apply correction at corner */ idx = ( xSteps - 1 ) * xSlice; asceFsens->xyz[Q_X] += table[ idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += table[ idx ].xyz[Q_Z]; q_invert( sensFsrod, table[ idx ].quat ); q_mult( asceFsens->quat, asceFsens->quat, sensFsrod ); q_normalize( asceFsens->quat, asceFsens->quat ); break; case MAX_Z: /* outside +X, -Y, and +Z */ /* apply correction at corner */ idx = ( xSteps - 1 ) * xSlice + ( zSteps - 1 ); asceFsens->xyz[Q_X] += table[ idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += table[ idx ].xyz[Q_Z]; q_invert( sensFsrod, table[ idx ].quat ); q_mult( asceFsens->quat, asceFsens->quat, sensFsrod ); q_normalize( asceFsens->quat, asceFsens->quat ); break; } /* end switch( possible Z locations ) */ } else if( code & MAX_Y ) { code &= ~MAX_Y; switch( code ) { case CTR: /* outside +X, +Y, center in Z */ z = ( asceFsens->xyz[Q_Z] - minCell[Q_Z] ) / cellIncr[Q_Z]; iZ = (int) z; fz = z - (double) iZ; mz = 1.0 - fz; idx = ( xSteps - 1 ) * xSlice + ( ySteps - 1 ) * ySlice + iZ; /* ** The reading projects to an edge of the table, bounded by ** table[xSteps-1][ySteps-1][iZ], table[xSteps-1][ySteps-1][iZ+1] */ /* lerp position */ asceFsens->xyz[Q_X] += mz * table[ idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += mz * table[ idx ].xyz[Q_X]; asceFsens->xyz[Q_Z] += mz * table[ idx ].xyz[Q_X]; asceFsens->xyz[Q_X] += fz * table[ ++idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += fz * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += fz * table[ idx ].xyz[Q_Z]; /* slerp orientation */ idx = ( xSteps - 1 ) * xSlice + ( ySteps - 1 ) * ySlice + iZ; q_slerp( srodFsens, table[ idx ].quat, table[ idx+1 ].quat, fz ); q_invert( sensFsrod, srodFsens ); q_mult( asceFsens->quat, asceFsens->quat, sensFsrod ); q_normalize( asceFsens->quat, asceFsens->quat ); break; case MIN_Z: /* outside +X, +Y, and -Z */ /* apply correction at corner */ idx = ( xSteps - 1 ) * xSlice + ( ySteps - 1 ) * ySlice; asceFsens->xyz[Q_X] += table[ idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += table[ idx ].xyz[Q_Z]; q_invert( sensFsrod, table[ idx ].quat ); q_mult( asceFsens->quat, asceFsens->quat, sensFsrod ); q_normalize( asceFsens->quat, asceFsens->quat ); break; case MAX_Z: /* outside +X, +Y, and +Z */ /* apply correction at corner */ idx = ( xSteps - 1 ) * xSlice + ( ySteps - 1 ) * ySlice + ( zSteps - 1 ); asceFsens->xyz[Q_X] += table[ idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += table[ idx ].xyz[Q_Z]; q_invert( sensFsrod, table[ idx ].quat ); q_mult( asceFsens->quat, asceFsens->quat, sensFsrod ); q_normalize( asceFsens->quat, asceFsens->quat ); break; } /* end switch of possible Z locations */ } else if( code & MIN_Z ) { /* We are outside +X, center in Y, and outside -Z */ y = ( asceFsens->xyz[Q_Y] - minCell[Q_Y] ) / cellIncr[Q_Y]; iY = (int) y; fy = y - (double) iY; my = 1.0 - fy; idx = ( xSteps - 1 ) * xSlice + iY * ySlice; /* lerp position */ asceFsens->xyz[Q_X] += my * table[ idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += my * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += my * table[ idx ].xyz[Q_Z]; asceFsens->xyz[Q_X] += fy * table[ idx += ySlice ].xyz[Q_X]; asceFsens->xyz[Q_Y] += fy * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += fy * table[ idx ].xyz[Q_Z]; /* slerp orientation */ idx = ( xSteps - 1 ) * xSlice + iY * ySlice; q_slerp( srodFsens, table[idx].quat, table[idx+ySlice].quat, fy ); q_invert( sensFsrod, srodFsens ); q_mult( asceFsens->quat, asceFsens->quat, sensFsrod ); q_normalize( asceFsens->quat, asceFsens->quat ); } else if( code & MAX_Z ) { /* had better be true by now */ /* Outside +X and +Z, similarly to previous case */ y = ( asceFsens->xyz[Q_Y] - minCell[Q_Y] ) / cellIncr[Q_Y]; iY = (int) y; fy = y - (double) iY; my = 1.0 - fy; idx = ( xSteps - 1 ) * xSlice + iY * ySlice + ( zSteps - 1 ); /* lerp position */ asceFsens->xyz[Q_X] += my * table[ idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += my * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += my * table[ idx ].xyz[Q_Z]; asceFsens->xyz[Q_X] += fy * table[ idx += ySlice ].xyz[Q_X]; asceFsens->xyz[Q_Y] += fy * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += fy * table[ idx ].xyz[Q_Z]; /* slerp orientation */ idx = ( xSteps - 1 ) * xSlice + iY * ySlice + ( zSteps - 1 ); q_slerp( srodFsens, table[idx].quat, table[idx+ySlice].quat, fy ); q_invert( sensFsrod, srodFsens ); q_mult( asceFsens->quat, asceFsens->quat, sensFsrod ); q_normalize( asceFsens->quat, asceFsens->quat ); } /* end cases for outside +X and something else */ } /* end outside +X and something else */ /* ** We now know that we are centered in X. ** We have eight cases left: MIN_Y, MAX_Y, MIN_Z, MAX_Z, ** MIN_Y & MIN_Z, MIN_Y & MAX_Z, MAX_Y & MIN_Z, MAX_Y & MAX_Z. */ } else if( code & MIN_Y ) { code &= ~MIN_Y; switch( code ) { case CTR: /* we are center in X, outside -Y, center in Z */ x = ( asceFsens->xyz[Q_X] - minCell[Q_X] ) / cellIncr[Q_X]; z = ( asceFsens->xyz[Q_Z] - minCell[Q_Z] ) / cellIncr[Q_Z]; iX = (int) x; fx = x - (double) iX; mx = 1.0 - fx; iZ = (int) z; fz = z - (double) iZ; mz = 1.0 - fz; fxfz = fx * fz; fxmz = fz * mz; mxfz = mx * fz; mxmz = mx * mz; idx = iX * xSlice + iZ; /* bilerp position */ asceFsens->xyz[Q_X] += mxmz * table[ idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += mxmz * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += mxmz * table[ idx ].xyz[Q_Z]; asceFsens->xyz[Q_X] += mxfz * table[ ++idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += mxfz * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += mxfz * table[ idx ].xyz[Q_Z]; asceFsens->xyz[Q_X] += fxfz * table[ idx += xSlice ].xyz[Q_X]; asceFsens->xyz[Q_Y] += fxfz * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += fxfz * table[ idx ].xyz[Q_Z]; asceFsens->xyz[Q_X] += fxmz * table[ --idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += fxmz * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += fxmz * table[ idx ].xyz[Q_Z]; /* bislerp orientation */ idx = iX * xSlice + iZ; q_slerp( Lx, table[ idx ].quat, table[ idx+1 ].quat, fz ); idx += xSlice; q_slerp( Ux, table[ idx ].quat, table[ idx+1 ].quat, fz ); q_slerp( srodFsens, Lx, Ux, fx ); q_invert( sensFsrod, srodFsens ); q_mult( asceFsens->quat, asceFsens->quat, sensFsrod ); q_normalize( asceFsens->quat, asceFsens->quat ); break; case MIN_Z: /* we are center in X, outside -Y and -Z */ x = ( asceFsens->xyz[Q_X] - minCell[Q_X] ) / cellIncr[Q_X]; iX = (int) x; fx = x - (double) iX; mx = 1.0 - fx; idx = iX * xSlice; /* lerp position */ asceFsens->xyz[Q_X] += mx * table[ idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += mx * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += mx * table[ idx ].xyz[Q_Z]; asceFsens->xyz[Q_X] += fx * table[ idx += xSlice ].xyz[Q_X]; asceFsens->xyz[Q_Y] += fx * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += fx * table[ idx ].xyz[Q_Z]; /* slerp orientation */ idx = iX * xSlice; q_slerp( srodFsens, table[idx].quat, table[ idx+xSlice ].quat, fx ); q_invert( sensFsrod, srodFsens ); q_mult( asceFsens->quat, asceFsens->quat, sensFsrod ); q_normalize( asceFsens->quat, asceFsens->quat ); break; case MAX_Z: /* we are center in X, outside -Y and +Z */ x = ( asceFsens->xyz[Q_X] - minCell[Q_X] ) / cellIncr[Q_X]; iX = (int) x; fx = x - (double) iX; mx = 1.0 - fx; idx = iX * xSlice + ( zSteps - 1 ); /* lerp position */ asceFsens->xyz[Q_X] += mx * table[ idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += mx * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += mx * table[ idx ].xyz[Q_Z]; asceFsens->xyz[Q_X] += fx * table[ idx += xSlice ].xyz[Q_X]; asceFsens->xyz[Q_Y] += fx * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += fx * table[ idx ].xyz[Q_Z]; /* slerp orientation */ idx = iX * xSlice + ( zSteps - 1 ); q_slerp( srodFsens, table[idx].quat, table[ idx+xSlice ].quat, fx ); q_invert( sensFsrod, srodFsens ); q_mult( asceFsens->quat, asceFsens->quat, sensFsrod ); q_normalize( asceFsens->quat, asceFsens->quat ); break; } /* end switch (possible cases of ctr X, -Y) */ } else if( code & MAX_Y ) { code &= ~MAX_Y; switch( code ) { case CTR: /* We are center in X, +Y, center in Z */ x = ( asceFsens->xyz[Q_X] - minCell[Q_X] ) / cellIncr[Q_X]; z = ( asceFsens->xyz[Q_Z] - minCell[Q_Z] ) / cellIncr[Q_Z]; iX = (int) x; fx = x - (double) iX; mx = 1.0 - fx; iZ = (int) z; fz = z - (double) iZ; mz = 1.0 - fz; fxfz = fx * fz; fxmz = fz * mz; mxfz = mx * fz; mxmz = mx * mz; idx = iX * xSlice + ( ySteps - 1 ) * ySlice + iZ; /* bilerp position */ asceFsens->xyz[Q_X] += mxmz * table[ idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += mxmz * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += mxmz * table[ idx ].xyz[Q_Z]; asceFsens->xyz[Q_X] += mxfz * table[ ++idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += mxfz * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += mxfz * table[ idx ].xyz[Q_Z]; asceFsens->xyz[Q_X] += fxfz * table[ idx += xSlice ].xyz[Q_X]; asceFsens->xyz[Q_Y] += fxfz * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += fxfz * table[ idx ].xyz[Q_Z]; asceFsens->xyz[Q_X] += fxmz * table[ --idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += fxmz * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += fxmz * table[ idx ].xyz[Q_Z]; /* bislerp orientation */ idx = iX * xSlice + ( ySteps - 1 ) * ySlice + iZ; q_slerp( Lx, table[ idx ].quat, table[ idx+1 ].quat, fz ); idx += xSlice; q_slerp( Ux, table[ idx ].quat, table[ idx+1 ].quat, fz ); q_slerp( srodFsens, Lx, Ux, fx ); q_invert( sensFsrod, srodFsens ); q_mult( asceFsens->quat, asceFsens->quat, sensFsrod ); q_normalize( asceFsens->quat, asceFsens->quat ); break; case MIN_Z: /* We are center in X, outside +Y and -Z */ x = ( asceFsens->xyz[Q_X] - minCell[Q_X] ) / cellIncr[Q_X]; iX = (int) x; fx = x - (double) iX; mx = 1.0 - fx; idx = iX * xSlice + ( ySteps - 1 ) * ySlice; /* lerp position */ asceFsens->xyz[Q_X] += mx * table[ idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += mx * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += mx * table[ idx ].xyz[Q_Z]; asceFsens->xyz[Q_X] += fx * table[ idx += xSlice ].xyz[Q_X]; asceFsens->xyz[Q_Y] += fx * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += fx * table[ idx ].xyz[Q_Z]; /* slerp orientation */ idx = iX * xSlice + ( ySteps - 1 ) * ySlice; q_slerp( srodFsens, table[idx].quat, table[ idx+xSlice ].quat, fx ); q_invert( sensFsrod, srodFsens ); q_mult( asceFsens->quat, asceFsens->quat, sensFsrod ); q_normalize( asceFsens->quat, asceFsens->quat ); break; case MAX_Z: /* We are center in X, outside +Y and +Z */ x = ( asceFsens->xyz[Q_X] - minCell[Q_X] ) / cellIncr[Q_X]; iX = (int) x; fx = x - (double) iX; mx = 1.0 - fx; idx = iX * xSlice + ( ySteps - 1 ) * ySlice + ( zSteps - 1 ); /* lerp position */ asceFsens->xyz[Q_X] += mx * table[ idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += mx * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += mx * table[ idx ].xyz[Q_Z]; asceFsens->xyz[Q_X] += fx * table[ idx += xSlice ].xyz[Q_X]; asceFsens->xyz[Q_Y] += fx * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += fx * table[ idx ].xyz[Q_Z]; /* slerp orientation */ idx = iX * xSlice + ( ySteps - 1 ) * ySlice + ( zSteps - 1 ); q_slerp( srodFsens, table[idx].quat, table[ idx+xSlice ].quat, fx ); q_invert( sensFsrod, srodFsens ); q_mult( asceFsens->quat, asceFsens->quat, sensFsrod ); q_normalize( asceFsens->quat, asceFsens->quat ); break; } /* end switch ( possible cases of ctr X, +Y) */ /* We are centered in X and Y. Remaining cases: MIN_Z, MAX_Z. */ } else if( code & MIN_Z ) { x = ( asceFsens->xyz[Q_X] - minCell[Q_X] ) / cellIncr[Q_X]; y = ( asceFsens->xyz[Q_Y] - minCell[Q_Y] ) / cellIncr[Q_Y]; iX = (int) x; fx = x - (double) iX; mx = 1.0 - fx; iY = (int) y; fy = y - (double) iY; my = 1.0 - fy; fxfy = fx * fy; fxmy = fx * my; mxfy = mx * fy; mxmy = mx * my; idx = iX * xSlice + iY * ySlice; /* bilerp position */ asceFsens->xyz[Q_X] += mxmy * table[ idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += mxmy * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += mxmy * table[ idx ].xyz[Q_Z]; asceFsens->xyz[Q_X] += mxfy * table[ idx += ySlice ].xyz[Q_X]; asceFsens->xyz[Q_Y] += mxfy * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += mxfy * table[ idx ].xyz[Q_Z]; asceFsens->xyz[Q_X] += fxfy * table[ idx += xSlice ].xyz[Q_X]; asceFsens->xyz[Q_Y] += fxfy * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += fxfy * table[ idx ].xyz[Q_Z]; asceFsens->xyz[Q_X] += fxmy * table[ idx -= ySlice ].xyz[Q_X]; asceFsens->xyz[Q_Y] += fxmy * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += fxmy * table[ idx ].xyz[Q_Z]; /* bislerp orientation */ idx = iX * xSlice + iY * ySlice; q_slerp( Lx, table[ idx ].quat, table[ idx+ySlice ].quat, fy ); idx += xSlice; q_slerp( Ux, table[ idx ].quat, table[ idx+ySlice ].quat, fy ); q_slerp( srodFsens, Lx, Ux, fx ); q_invert( sensFsrod, srodFsens ); q_mult( asceFsens->quat, asceFsens->quat, sensFsrod ); q_normalize( asceFsens->quat, asceFsens->quat ); } else if( code & MAX_Z ) { /* had better be true by this point */ x = ( asceFsens->xyz[Q_X] - minCell[Q_X] ) / cellIncr[Q_X]; y = ( asceFsens->xyz[Q_Y] - minCell[Q_Y] ) / cellIncr[Q_Y]; iX = (int) x; fx = x - (double) iX; mx = 1.0 - fx; iY = (int) y; fy = y - (double) iY; my = 1.0 - fy; fxfy = fx * fy; fxmy = fx * my; mxfy = mx * fy; mxmy = mx * my; idx = iX * xSlice + iY * ySlice + ( zSteps - 1 ); /* bilerp position */ asceFsens->xyz[Q_X] += mxmy * table[ idx ].xyz[Q_X]; asceFsens->xyz[Q_Y] += mxmy * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += mxmy * table[ idx ].xyz[Q_Z]; asceFsens->xyz[Q_X] += mxfy * table[ idx += ySlice ].xyz[Q_X]; asceFsens->xyz[Q_Y] += mxfy * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += mxfy * table[ idx ].xyz[Q_Z]; asceFsens->xyz[Q_X] += fxfy * table[ idx += xSlice ].xyz[Q_X]; asceFsens->xyz[Q_Y] += fxfy * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += fxfy * table[ idx ].xyz[Q_Z]; asceFsens->xyz[Q_X] += fxmy * table[ idx -= ySlice ].xyz[Q_X]; asceFsens->xyz[Q_Y] += fxmy * table[ idx ].xyz[Q_Y]; asceFsens->xyz[Q_Z] += fxmy * table[ idx ].xyz[Q_Z]; /* bislerp orientation */ idx = iX * xSlice + iY * ySlice + ( zSteps - 1 ); q_slerp( Lx, table[ idx ].quat, table[ idx+ySlice ].quat, fy ); idx += xSlice; q_slerp( Ux, table[ idx ].quat, table[ idx+ySlice ].quat, fy ); q_slerp( srodFsens, Lx, Ux, fx ); q_invert( sensFsrod, srodFsens ); q_mult( asceFsens->quat, asceFsens->quat, sensFsrod ); q_normalize( asceFsens->quat, asceFsens->quat ); } } /* end outside table -- extrapolate */ return retcode; /* The outcode of the original octant */ } /* The following routine is a hack for displaying the table efficiently */ /* ** Fills in a list of points that represent the grid cells and the grid values ** that should be displayed. The arguments should be the addresses of ** unallocated pointers. This routine allocates space and assigns these ** pointers to point to it. No return value, but the two pointers will be ** NULL if the space could not be allocated. */ int tabCreateDisplayLists( q_vec_type **g, q_vec_type **v, q_vec_type *a[3] ) { int i, j, k, tableCells, idx, ptCount; double x, y, z; q_matrix_type tempmat; /* Allocate space */ tableCells = xSteps * ySteps * zSteps; *g = NEWVEC( tableCells ); if( *g == NULL ) { *v = NULL; return 0; } *v = NEWVEC( tableCells ); if( *v == NULL ) { free( *g ); *g = NULL; return 0; } a[Q_X] = NEWVEC( tableCells ); if( a[Q_X] == NULL ) { free( *g ); *g = NULL; free( *v ); *v = NULL; return 0; } a[Q_Y] = NEWVEC( tableCells ); if( a[Q_Y] == NULL ) { free( *g ); *g = NULL; free( *v ); *v = NULL; free( a[Q_X] ); a[Q_X] = NULL; return 0; } a[Q_Z] = NEWVEC( tableCells ); if( a[Q_Z] == NULL ) { free( *g ); *g = NULL; free( *v ); *v = NULL; free( a[Q_X] ); a[Q_X] = NULL; free( a[Q_Y] ); a[Q_Y] = NULL; return 0; } for( ptCount = 0, i = 0; i < xSteps; i++ ) for( j = 0; j < ySteps; j++ ) for( k = 0; k < zSteps; k++, ptCount++ ) { if( xSteps == 1 ) x = minCell[Q_X]; else x = minCell[Q_X] + i * ( maxCell[Q_X] - minCell[Q_X] ) / (double) ( xSteps - 1 ); if( ySteps == 1 ) y = minCell[Q_Y]; else y = minCell[Q_Y] + j * ( maxCell[Q_Y] - minCell[Q_Y] ) / (double) ( ySteps - 1 ); if( zSteps == 1 ) z = minCell[Q_Z]; else z = minCell[Q_Z] + k * ( maxCell[Q_Z] - minCell[Q_Z] ) / (double) ( zSteps - 1 ); idx = INDEX( i, j, k, xSteps, ySteps, zSteps ); /* following assumes full table */ q_set_vec( (*g)[ ptCount ], x, y, z ); #ifdef ABS_POSN q_vec_copy( (*v)[ ptCount ], table[ idx ].xyz ); #else q_vec_add( (*v)[ ptCount ], (*g)[ ptCount ], table[ idx ].xyz ); #endif q_to_row_matrix( tempmat, table[ ptCount ].quat ); qx_vec_mult_matrix( a[Q_X][ ptCount ], Xaxis, tempmat ); qx_vec_mult_matrix( a[Q_Y][ ptCount ], Yaxis, tempmat ); qx_vec_mult_matrix( a[Q_Z][ ptCount ], Zaxis, tempmat ); } return tableCells; } /* ** The following routine is a test that analyzes the resolution of the table. ** The procedure is as follows: for each (x,y,z) tuple of odd indices between ** (3,3,3) and (x-3,y-3,z-3), determine the difference between the grid value ** read and the value interpolated from the surrounding even-tupled cells. */ void tabAnalyzeRes( void ) { int xI, yI, zI, idx, nPts = 0; double dist, totDist = 0.0, ang, totAng = 0.0; q_vec_type interpV; q_type interpQ, LxLy, LxUy, UxLy, UxUy, Lx, Ux, diffQ; for( xI = 3; xI < xSteps - 3; xI += 2 ) for( yI = 3; yI < ySteps - 3; yI += 2 ) for( zI = 3; zI < zSteps - 3; zI += 2, nPts++ ) { /* Interpolate this position value */ idx = INDEX( xI-1, yI-1, zI-1, xSteps, ySteps, zSteps ); q_vec_copy( interpV, table[ idx ].xyz ); idx += 2; q_vec_add( interpV, interpV, table[ idx ].xyz ); idx += ySlice * 2; q_vec_add( interpV, interpV, table[ idx ].xyz ); idx -= 2; q_vec_add( interpV, interpV, table[ idx ].xyz ); idx += xSlice * 2; q_vec_add( interpV, interpV, table[ idx ].xyz ); idx += 2; q_vec_add( interpV, interpV, table[ idx ].xyz ); idx -= ySlice * 2; q_vec_add( interpV, interpV, table[ idx ].xyz ); idx -= 2; q_vec_add( interpV, interpV, table[ idx ].xyz ); q_vec_scale( interpV, 0.125, interpV ); /* Compute magnitude of error from real grid point */ idx = INDEX( xI, yI, zI, xSteps, ySteps, zSteps ); dist = q_vec_distance( interpV, table[ idx ].xyz ); /* Accumulate position error */ totDist += dist; /* Interpolate this orientation value */ idx = INDEX( xI-1, yI-1, zI-1, xSteps, ySteps, zSteps ); q_slerp( LxLy, table[ idx ].quat, table[ idx + 2 ].quat, 0.5 ); idx += ySlice * 2; q_slerp( LxUy, table[ idx ].quat, table[ idx + 2 ].quat, 0.5 ); idx += xSlice * 2; q_slerp( UxUy, table[ idx ].quat, table[ idx + 2 ].quat, 0.5 ); idx -= ySlice * 2; q_slerp( UxLy, table[ idx ].quat, table[ idx + 2 ].quat, 0.5 ); q_slerp( Lx, LxLy, LxUy, 0.5 ); q_slerp( Ux, UxLy, UxUy, 0.5 ); q_slerp( interpQ, Lx, Ux, 0.5 ); /* Compute half-angle of error from real grid point */ idx = INDEX( xI, yI, zI, xSteps, ySteps, zSteps ); qx_diff( diffQ, interpQ, table[ idx ].quat ); if( diffQ[ Q_W ] < 1.0 ) { ang = acos( diffQ[ Q_W ] ); /* Accumulate orientation error */ totAng += ang; } } printf( "Average position error of intermediate grid points = %8.6lf\n", totDist / nPts ); printf( "Average angular error of intermediate grid points = %8.6lf\n", Q_RAD_TO_DEG( 2.0 * totAng / nPts ) ); return; }