head 1.3; access; symbols; locks; strict; comment @# @; 1.3 date 2002.09.07.01.50.35; author curt; state Exp; branches; next 1.2; 1.2 date 2002.08.31.19.01.50; author curt; state Exp; branches; next 1.1; 1.1 date 2002.08.31.18.53.58; author curt; state Exp; branches; next ; desc @@ 1.3 log @Updated doxygen docs. @ text @
Main Page Class Hierarchy Compound List File List Compound Members File Members

# MAT3inv.c

    00001 /* Copyright 1988, Brown Computer Graphics Group.  All Rights Reserved. */
    00002
    00003 /* --------------------------------------------------------------------------
    00004  * This file contains routines that operate solely on matrices.
    00005  * -------------------------------------------------------------------------*/
    00006
    00007 #include "mat3defs.h"
    00008
    00009 /* --------------------------  Static Routines  ---------------------------- */
    00010
    00011 #define SMALL 1e-20             /* Small enough to be considered zero */
    00012
    00013 /*
    00014  * Shuffles rows in inverse of 3x3.  See comment in MAT3_inv3_second_col().
    00015  */
    00016
    00017 static void
    00018 MAT3_inv3_swap( register double inv[3][3], int row0, int row1, int row2)
    00019 {
    00020    register int i, tempi;
    00021    double       temp;
    00022
    00023 #define SWAP_ROWS(a, b) \
    00024    for (i = 0; i < 3; i++) SWAP(inv[a][i], inv[b][i], temp); \
    00025    SWAP(a, b, tempi)
    00026
    00027    if (row0 != 0){
    00028       if (row1 == 0) {
    00029          SWAP_ROWS(row0, row1);
    00030       }
    00031       else {
    00032          SWAP_ROWS(row0, row2);
    00033       }
    00034    }
    00035
    00036    if (row1 != 1) {
    00037       SWAP_ROWS(row1, row2);
    00038    }
    00039 }
    00040
    00041 /*
    00042  * Does Gaussian elimination on second column.
    00043  */
    00044
    00045 static int
    00046 MAT3_inv3_second_col (register double source[3][3], register double inv[3][3], int row0)
    00047 {
    00048    register int row1, row2, i1, i2, i;
    00049    double       temp;
    00050    double       a, b;
    00051
    00052    /* Find which row to use */
    00053    if      (row0 == 0)  i1 = 1, i2 = 2;
    00054    else if (row0 == 1)  i1 = 0, i2 = 2;
    00055    else                 i1 = 0, i2 = 1;
    00056
    00057    /* Find which is larger in abs. val.:the entry in [i1][1] or [i2][1] */
    00058    /* and use that value for pivoting.                                  */
    00059
    00060    a = source[i1][1]; if (a < 0) a = -a;
    00061    b = source[i2][1]; if (b < 0) b = -b;
    00062    if (a > b)   row1 = i1;
    00063    else         row1 = i2;
    00064    row2 = (row1 == i1 ? i2 : i1);
    00065
    00066    /* Scale row1 in source */
    00067    if ((source[row1][1] < SMALL) && (source[row1][1] > -SMALL)) return(FALSE);
    00068    temp = 1.0 / source[row1][1];
    00069    source[row1][1]  = 1.0;
    00070    source[row1][2] *= temp;     /* source[row1][0] is zero already */
    00071
    00072    /* Scale row1 in inv */
    00073    inv[row1][row1]  = temp;     /* it used to be a 1.0 */
    00074    inv[row1][row0] *= temp;
    00075
    00076    /* Clear column one, source, and make corresponding changes in inv */
    00077
    00078    for (i = 0; i < 3; i++) if (i != row1) {     /* for i = all rows but row1 */
    00079    temp = -source[i][1];
    00080       source[i][1]  = 0.0;
    00081       source[i][2] += temp * source[row1][2];
    00082
    00083       inv[i][row1]  = temp * inv[row1][row1];
    00084       inv[i][row0] += temp * inv[row1][row0];
    00085    }
    00086
    00087    /* Scale row2 in source */
    00088    if ((source[row2][2] < SMALL) && (source[row2][2] > -SMALL)) return(FALSE);
    00089    temp = 1.0 / source[row2][2];
    00090    source[row2][2] = 1.0;       /* source[row2][*] is zero already */
    00091
    00092    /* Scale row2 in inv */
    00093    inv[row2][row2]  = temp;     /* it used to be a 1.0 */
    00094    inv[row2][row0] *= temp;
    00095    inv[row2][row1] *= temp;
    00096
    00097    /* Clear column one, source, and make corresponding changes in inv */
    00098    for (i = 0; i < 3; i++) if (i != row2) {     /* for i = all rows but row2 */
    00099    temp = -source[i][2];
    00100       source[i][2] = 0.0;
    00101       inv[i][row0] += temp * inv[row2][row0];
    00102       inv[i][row1] += temp * inv[row2][row1];
    00103       inv[i][row2] += temp * inv[row2][row2];
    00104    }
    00105
    00106    /*
    00107     * Now all is done except that the inverse needs to have its rows shuffled.
    00108     * row0 needs to be moved to inv[0][*], row1 to inv[1][*], etc.
    00109     *
    00110     * We *didn't* do the swapping before the elimination so that we could more
    00111     * easily keep track of what ops are needed to be done in the inverse.
    00112     */
    00113    MAT3_inv3_swap(inv, row0, row1, row2);
    00114
    00115    return(TRUE);
    00116 }
    00117
    00118 /*
    00119  * Fast inversion routine for 3 x 3 matrices.   - Written by jfh.
    00120  *
    00121  * This takes 30 multiplies/divides, as opposed to 39 for Cramer's Rule.
    00122  * The algorithm consists of performing fast gaussian elimination, by never
    00123  * doing any operations where the result is guaranteed to be zero, or where
    00124  * one operand is guaranteed to be zero. This is done at the cost of clarity,
    00125  * alas.
    00126  *
    00127  * Returns 1 if the inverse was successful, 0 if it failed.
    00128  */
    00129
    00130 static int
    00131 MAT3_invert3 (register double source[3][3], register double inv[3][3])
    00132 {
    00133    register int i, row0;
    00134    double       temp;
    00135    double       a, b, c;
    00136
    00137    inv[0][0] = inv[1][1] = inv[2][2] = 1.0;
    00138    inv[0][1] = inv[0][2] = inv[1][0] = inv[1][2] = inv[2][0] = inv[2][1] = 0.0;
    00139
    00140    /* attempt to find the largest entry in first column to use as pivot */
    00141    a = source[0][0]; if (a < 0) a = -a;
    00142    b = source[1][0]; if (b < 0) b = -b;
    00143    c = source[2][0]; if (c < 0) c = -c;
    00144
    00145    if (a > b) {
    00146       if (a > c) row0 = 0;
    00147       else row0 = 2;
    00148    }
    00149    else {
    00150       if (b > c) row0 = 1;
    00151       else row0 = 2;
    00152    }
    00153
    00154    /* Scale row0 of source */
    00155    if ((source[row0][0] < SMALL) && (source[row0][0] > -SMALL)) return(FALSE);
    00156    temp = 1.0 / source[row0][0];
    00157    source[row0][0]  = 1.0;
    00158    source[row0][1] *= temp;
    00159    source[row0][2] *= temp;
    00160
    00161    /* Scale row0 of inverse */
    00162    inv[row0][row0] = temp;      /* other entries are zero -- no effort  */
    00163
    00164    /* Clear column zero of source, and make corresponding changes in inverse */
    00165
    00166    for (i = 0; i < 3; i++) if (i != row0) {     /* for i = all rows but row0 */
    00167       temp = -source[i][0];
    00168       source[i][0]  = 0.0;
    00169       source[i][1] += temp * source[row0][1];
    00170       source[i][2] += temp * source[row0][2];
    00171       inv[i][row0]  = temp * inv[row0][row0];
    00172    }
    00173
    00174    /*
    00175     * We've now done gaussian elimination so that the source and
    00176     * inverse look like this:
    00177     *
    00178     *   1 * *           * 0 0
    00179     *   0 * *           * 1 0
    00180     *   0 * *           * 0 1
    00181     *
    00182     * We now proceed to do elimination on the second column.
    00183     */
    00184    if (! MAT3_inv3_second_col(source, inv, row0)) return(FALSE);
    00185
    00186    return(TRUE);
    00187 }
    00188
    00189 /*
    00190  * Finds a new pivot for a non-simple 4x4.  See comments in MAT3invert().
    00191  */
    00192
    00193 static int
    00194 MAT3_inv4_pivot (register MAT3mat src, MAT3vec r, double *s, int *swap)
    00195 {
    00196    register int i, j;
    00197    double       temp, max;
    00198
    00199    *swap = -1;
    00200
    00201    if (MAT3_IS_ZERO(src[3][3])) {
    00202
    00203       /* Look for a different pivot element: one with largest abs value */
    00204       max = 0.0;
    00205
    00206       for (i = 0; i < 4; i++) {
    00207          if      (src[i][3] >  max) max =  src[*swap = i][3];
    00208          else if (src[i][3] < -max) max = -src[*swap = i][3];
    00209       }
    00210
    00211       /* No pivot element available ! */
    00212       if (*swap < 0) return(FALSE);
    00213
    00214       else for (j = 0; j < 4; j++) SWAP(src[*swap][j], src[3][j], temp);
    00215    }
    00216
    00217    MAT3_SET_VEC (r, -src[0][3], -src[1][3], -src[2][3]);
    00218
    00219    *s = 1.0 / src[3][3];
    00220
    00221    src[0][3] = src[1][3] = src[2][3] = 0.0;
    00222    src[3][3]                         = 1.0;
    00223
    00224    MAT3_SCALE_VEC(src[3], src[3], *s);
    00225
    00226    for (i = 0; i < 3; i++) {
    00227       src[0][i] += r[0] * src[3][i];
    00228       src[1][i] += r[1] * src[3][i];
    00229       src[2][i] += r[2] * src[3][i];
    00230    }
    00231
    00232    return(TRUE);
    00233 }
    00234
    00235 /* -------------------------  Internal Routines  --------------------------- */
    00236
    00237 /* --------------------------  Public Routines  ---------------------------- */
    00238
    00239 /*
    00240  * This returns the inverse of the given matrix.  The result matrix
    00241  * may be the same as the one to invert.
    00242  *
    00243  * Fast inversion routine for 4 x 4 matrices, written by jfh.
    00244  *
    00245  * Returns 1 if the inverse was successful, 0 if it failed.
    00246  *
    00247  * This routine has been specially tweaked to notice the following:
    00248  * If the matrix has the form
    00249  *        * * * 0
    00250  *        * * * 0
    00251  *        * * * 0
    00252  *        * * * 1
    00253  *
    00254  * (as do many matrices in graphics), then we compute the inverse of
    00255  * the upper left 3x3 matrix and use this to find the general inverse.
    00256  *
    00257  *   In the event that the right column is not 0-0-0-1, we do gaussian
    00258  * elimination to make it so, then use the 3x3 inverse, and then do
    00259  * our gaussian elimination.
    00260  */
    00261
    00262 int
    00263 MAT3invert(double (*result_mat)[4], double (*mat)[4])
    00264 {
    00265    MAT3mat              src, inv;
    00266    register int         i, j, simple;
    00267    double               m[3][3], inv3[3][3], s, temp;
    00268    MAT3vec              r, t;
    00269    int                  swap;
    00270
    00271    MAT3copy(src, mat);
    00272    MAT3identity(inv);
    00273
    00274    /* If last column is not (0,0,0,1), use special code */
    00275    simple = (mat[0][3] == 0.0 && mat[1][3] == 0.0 &&
    00276              mat[2][3] == 0.0 && mat[3][3] == 1.0);
    00277
    00278    if (! simple && ! MAT3_inv4_pivot(src, r, &s, &swap)) return(FALSE);
    00279
    00280    MAT3_COPY_VEC(t, src[3]);    /* Translation vector */
    00281
    00282    /* Copy upper-left 3x3 matrix */
    00283    for (i = 0; i < 3; i++) for (j = 0; j < 3; j++) m[i][j] = src[i][j];
    00284
    00285    if (! MAT3_invert3(m, inv3)) return(FALSE);
    00286
    00287    for (i = 0; i < 3; i++) for (j = 0; j < 3; j++) inv[i][j] = inv3[i][j];
    00288
    00289    for (i = 0; i < 3; i++) for (j = 0; j < 3; j++)
    00290       inv[3][i] -= t[j] * inv3[j][i];
    00291
    00292    if (! simple) {
    00293
    00294       /* We still have to undo our gaussian elimination from earlier on */
    00295       /* add r0 * first col to last col */
    00296       /* add r1 * 2nd   col to last col */
    00297       /* add r2 * 3rd   col to last col */
    00298
    00299       for (i = 0; i < 4; i++) {
    00300          inv[i][3] += r[0] * inv[i][0] + r[1] * inv[i][1] + r[2] * inv[i][2];
    00301          inv[i][3] *= s;
    00302       }
    00303
    00304       if (swap >= 0)
    00305          for (i = 0; i < 4; i++) SWAP(inv[i][swap], inv[i][3], temp);
    00306    }
    00307
    00308    MAT3copy(result_mat, inv);
    00309
    00310    return(TRUE);
    00311 }

@ 1.2 log @Oops, try again. @ text @d320 1 a320 1