home *** CD-ROM | disk | FTP | other *** search
/ Encyclopedia of Graphics File Formats Companion / GFF_CD.ISO / software / unix / saoimage / sao1_07.tar / crdinvrt.c < prev    next >
C/C++ Source or Header  |  1990-04-20  |  6KB  |  247 lines
  1. #ifndef lint
  2. static char SccsId[] = "%W%  %G%";
  3. #endif
  4.  
  5. /* Module:    crdinvrt.c (Coordinate Invert)
  6.  * Purpose:    Compute reverse transform and unknown transform parameters
  7.  * Subroutine:    invert_matrix()            returns: void
  8.  * Subroutine:    compute_iadd_invert()        returns: void
  9.  * Xlib calls:    none
  10.  * Copyright:    1988 Smithsonian Astrophysical Observatory
  11.  *        You may do anything you like with this file except remove
  12.  *        this copyright.  The Smithsonian Astrophysical Observatory
  13.  *        makes no representations about the suitability of this
  14.  *        software for any purpose.  It is provided "as is" without
  15.  *        express or implied warranty.
  16.  * Modified:    {0} Michael VanHilst    initial version            16 March 1988
  17.  *        {n} <who> -- <does what> -- <when>
  18.  */
  19.  
  20. #include <stdio.h>        /* stderr, NULL, etc */
  21. #include <math.h>        /* get trig functions, sqrt, and fabs */
  22. #include "hfiles/coord.h"    /* Transform and other coord structs */
  23.  
  24. #define N 3    /* all matrices are 3x3 */
  25.  
  26. /*
  27.  * Subroutine:    invert_matrix
  28.  * Purpose:    Compute parameters of the inverse transform
  29.  * Method:    Uses LU decomposition method
  30.  */
  31. void invert_matrix ( old, new )
  32.      Transform *old, *new;
  33. {
  34.   float scratch[3][3], column[3];
  35.   int pivots[3];
  36.   static void lu_decompose(), lu_backsub();
  37.  
  38.   scratch[0][0] = old->inx_outx;
  39.   scratch[1][0] = old->iny_outx;
  40.   scratch[2][0] = old->add_outx;
  41.   scratch[0][1] = old->inx_outy;
  42.   scratch[1][1] = old->iny_outy;
  43.   scratch[2][1] = old->add_outy;
  44.   scratch[0][2] = scratch[1][2] = 0.0;
  45.   scratch[2][2] = 1.0;
  46.   lu_decompose(scratch, pivots);
  47.   /* solve for out X */
  48.   column[1] = column[2] = 0.0;
  49.   column[0] = 1.0;
  50.   lu_backsub(scratch, pivots, column);
  51.   new->inx_outx = column[0];
  52.   new->iny_outx = column[1];
  53.   new->add_outx = column[2];
  54.   /* solve for out Y */
  55.   column[0] = column[2] = 0.0;
  56.   column[1] = 1.0;
  57.   lu_backsub(scratch, pivots, column);
  58.   new->inx_outy = column[0];
  59.   new->iny_outy = column[1];
  60.   new->add_outy = column[2];
  61. }
  62.  
  63. /*
  64.  * Subroutine:    compute_iadd_invert
  65.  * Purpose:    Compute the offsets used for integer transforms
  66.  * Method:    Uses matrix inversion
  67.  */
  68. void compute_iadd_invert ( old, new, ioff )
  69.      Transform *old, *new;
  70.      float ioff;
  71. {
  72.   float scratch[3][3], column[3];
  73.   int pivots[3];
  74.   static void lu_decompose(), lu_backsub();
  75.  
  76.   /* set transform equations in matrix form */
  77.   scratch[0][0] = old->inx_outx;
  78.   scratch[1][0] = old->iny_outx;
  79.   scratch[2][0] = old->add_outx + ioff;
  80.   scratch[0][1] = old->inx_outy;
  81.   scratch[1][1] = old->iny_outy;
  82.   scratch[2][1] = old->add_outy + ioff;
  83.   scratch[0][2] = scratch[1][2] = 0.0;
  84.   scratch[2][2] = 1.0;
  85.   lu_decompose(scratch, pivots);
  86.   /* solve for out X */
  87.   column[1] = column[2] = 0.0;
  88.   column[0] = 1.0;
  89.   lu_backsub(scratch, pivots, column);
  90.   new->iadd_outx = column[2];
  91.   /* solve for out Y */
  92.   column[0] = column[2] = 0.0;
  93.   column[1] = 1.0;
  94.   lu_backsub(scratch, pivots, column);
  95.   new->iadd_outy = column[2];
  96. }
  97.  
  98. /*
  99.  * Subroutine:    lu_decompose
  100.  * Purpose:    lower upper decompose matrix (in place) for matrix inversion
  101.  */
  102. static void lu_decompose ( mtrx, pivots )
  103.      float mtrx[3][3];
  104.      int pivots[3];
  105. {
  106.   int i, j, k, imax;
  107.   double sum, mag, column_max;
  108.   float merit;
  109.   float rescale[3];
  110.  
  111.   /* find the maximum values in each column */
  112.   for( i=0; i<N; i++ ) {
  113.     column_max = 0.0;
  114.     for( j=0; j<N; j++ ) {
  115.       mag = fabs((double)mtrx[i][j]);
  116.       if( mag > column_max ) column_max = mag;
  117.     }
  118.     /* save scaling value */
  119.     rescale[i] = 1.0 / column_max;
  120.   }
  121.   /* loop through columns for Crouts method */
  122.   for( j=0; j<N; j++ ) {
  123.     if( j > 0 ) {
  124.       for( i=0; i<j; i++ ) {
  125.     sum = mtrx[i][j];
  126.     if( i > 0 ) {
  127.       for( k=0; k<i; k++ ) {
  128.         sum = sum - (mtrx[i][k] * mtrx[k][j]);
  129.       }
  130.       mtrx[i][j] = sum;
  131.     }
  132.       }
  133.     }
  134.     /* initialize search for largest pivot element */
  135.     column_max = 0.0;
  136.     for( i=j; i<N; i++ ) {
  137.       sum = mtrx[i][j];
  138.       if( j > 0 ) {
  139.     for( k=0; k<j; k++ ) {
  140.       sum = sum - (mtrx[i][k] * mtrx[k][j]);
  141.     }
  142.     mtrx[i][j] = sum;
  143.       }
  144.       /* measurement of merit for pivot element */
  145.       merit = rescale[i] * fabs(sum);
  146.       if( merit >= column_max ) {
  147.     column_max = merit;
  148.     imax = i;
  149.       }
  150.     }
  151.     /* need to interchange rows? */
  152.     if( j != imax ) {
  153.       for( k=0; k<N; k++ ) {
  154.     merit = mtrx[imax][k];
  155.     mtrx[imax][k] = mtrx[j][k];
  156.     mtrx[j][k] = merit;
  157.       }
  158.       /* also interchange scale factor */
  159.       rescale[imax] = rescale[j];
  160.     }
  161.     pivots[j] = imax;
  162.     /* divide by pivot element */
  163.     if( j != N ) {
  164.       merit = 1.0 / mtrx[j][j];
  165.       for( i=j+1; i<N; i++ ) {
  166.     mtrx[i][j] = mtrx[i][j] * merit;
  167.       }
  168.     }
  169.   } /* repeat for next column */
  170. }
  171.  
  172. /*
  173.  * Subroutine:    lu_backsub
  174.  * Purpose:    LowerUpper backsubstitute one column to solve matrix inversion
  175.  */
  176. static void lu_backsub ( lumtrx, pivots, result )
  177.      float lumtrx[N][N];
  178.      int pivots[N];
  179.      float result[N];
  180. {
  181.   int index, pivot;
  182.   double sum;
  183.   int i, j;
  184.  
  185.   index = (-1);
  186.   /* do forward substitution and unscramble permutations */
  187.   for( i=0; i<N; i++ ) {
  188.     pivot = pivots[i];
  189.     sum = result[pivot];
  190.     result[pivot] = result[i];
  191.     if( index >=0 ) {
  192.       for( j=index; j<i; j++ ) {
  193.     sum = sum - (lumtrx[i][j] * result[j]);
  194.       }
  195.     }
  196.     /* start summing after first non-zero element */
  197.     else if( sum != 0 ) index = i;
  198.     result[i]= sum;
  199.   }
  200.   /* do back substitution */
  201.   for( i=N-1; i>=0; i-- ) {
  202.     sum = result[i];
  203.     if( i < (N-1) ) {
  204.       for( j=i+1; j<N; j++ ) {
  205.     sum = sum - (lumtrx[i][j] * result[j]);
  206.       }
  207.     }
  208.     /* store component of solution vector */
  209.     result[i] = sum / lumtrx[i][i];
  210.   }
  211. }
  212.  
  213. #ifdef SAMPLE
  214. /*
  215.  * invert a transform matrix, gives equations for opposite transform
  216.  */
  217. void invert_matrix ( mtrx, inverse )
  218.      float mtrx[N][N], inverse[N][N];
  219. {
  220.   float scratch[N][N], column[N];
  221.   int pivots[N];
  222.   int i, j;
  223.  
  224.   /* set up identity matrix and copy of matrix a */
  225.   for( i=0; i<N; i++ ) {
  226.     for( j=0; j<N; j++ ) {
  227.       inverse[i][j] = 0.0;
  228.       scratch[i][j] = mtrx[i][j];
  229.     }
  230.     inverse[i][i] = 1.0;
  231.   }
  232.   /* lower/upper decompose matrix just once */
  233.   lu_decompose ( scratch, pivots );
  234.   /* find inverse column by column */
  235.   for( j=0; j<N; j++ ) {
  236.     for( i=0; i<N; i++ ) {
  237.       column[i] = inverse[i][j];
  238.     }
  239.     /* backsubstitute one column at a time */
  240.     lu_backsub( scratch, pivots, column );
  241.     for( i=0; i<N; i++ ) {
  242.       inverse[i][j] = column[i];
  243.     }
  244.   }
  245. }
  246. #endif
  247.