Lib/Math/MAT3inv.c

   1 /* Copyright 1988, Brown Computer Graphics Group.  All Rights Reserved. */
   2
   3 /* --------------------------------------------------------------------------
   4  * This file contains routines that operate solely on matrices.
   5  * -------------------------------------------------------------------------*/
   6
   7 #include <Math/mat3defs.h>
   8
   9 /* --------------------------  Static Routines  ---------------------------- */
  10
  11 #define SMALL 1e-20             /* Small enough to be considered zero */
  12
  13 /*
  14  * Shuffles rows in inverse of 3x3.  See comment in MAT3_inv3_second_col().
  15  */
  16
  17 static void
  18 MAT3_inv3_swap( register double inv[3][3], int row0, int row1, int row2)
  19 {
  20    register int i, tempi;
  21    double       temp;
  22
  23 #define SWAP_ROWS(a, b) \
  24    for (i = 0; i < 3; i++) SWAP(inv[a][i], inv[b][i], temp); \
  25    SWAP(a, b, tempi)
  26
  27    if (row0 != 0){
  28       if (row1 == 0) {
  29          SWAP_ROWS(row0, row1);
  30       }
  31       else {
  32          SWAP_ROWS(row0, row2);
  33       }
  34    }
  35
  36    if (row1 != 1) {
  37       SWAP_ROWS(row1, row2);
  38    }
  39 }
  40
  41 /*
  42  * Does Gaussian elimination on second column.
  43  */
  44
  45 static int
  46 MAT3_inv3_second_col (register double source[3][3], register double inv[3][3], int row0)
  47 {
  48    register int row1, row2, i1, i2, i;
  49    double       temp;
  50    double       a, b;
  51
  52    /* Find which row to use */
  53    if      (row0 == 0)  i1 = 1, i2 = 2;
  54    else if (row0 == 1)  i1 = 0, i2 = 2;
  55    else                 i1 = 0, i2 = 1;
  56
  57    /* Find which is larger in abs. val.:the entry in [i1][1] or [i2][1] */
  58    /* and use that value for pivoting.                                  */
  59
  60    a = source[i1][1]; if (a < 0) a = -a;
  61    b = source[i2][1]; if (b < 0) b = -b;
  62    if (a > b)   row1 = i1;
  63    else         row1 = i2;
  64    row2 = (row1 == i1 ? i2 : i1);
  65
  66    /* Scale row1 in source */
  67    if ((source[row1][1] < SMALL) && (source[row1][1] > -SMALL)) return(FALSE);
  68    temp = 1.0 / source[row1][1];
  69    source[row1][1]  = 1.0;
  70    source[row1][2] *= temp;     /* source[row1][0] is zero already */
  71
  72    /* Scale row1 in inv */
  73    inv[row1][row1]  = temp;     /* it used to be a 1.0 */
  74    inv[row1][row0] *= temp;
  75
  76    /* Clear column one, source, and make corresponding changes in inv */
  77
  78    for (i = 0; i < 3; i++) if (i != row1) {     /* for i = all rows but row1 */
  79    temp = -source[i][1];
  80       source[i][1]  = 0.0;
  81       source[i][2] += temp * source[row1][2];
  82
  83       inv[i][row1]  = temp * inv[row1][row1];
  84       inv[i][row0] += temp * inv[row1][row0];
  85    }
  86
  87    /* Scale row2 in source */
  88    if ((source[row2][2] < SMALL) && (source[row2][2] > -SMALL)) return(FALSE);
  89    temp = 1.0 / source[row2][2];
  90    source[row2][2] = 1.0;       /* source[row2][*] is zero already */
  91
  92    /* Scale row2 in inv */
  93    inv[row2][row2]  = temp;     /* it used to be a 1.0 */
  94    inv[row2][row0] *= temp;
  95    inv[row2][row1] *= temp;
  96
  97    /* Clear column one, source, and make corresponding changes in inv */
  98    for (i = 0; i < 3; i++) if (i != row2) {     /* for i = all rows but row2 */
  99    temp = -source[i][2];
 100       source[i][2] = 0.0;
 101       inv[i][row0] += temp * inv[row2][row0];
 102       inv[i][row1] += temp * inv[row2][row1];
 103       inv[i][row2] += temp * inv[row2][row2];
 104    }
 105
 106    /*
 107     * Now all is done except that the inverse needs to have its rows shuffled.
 108     * row0 needs to be moved to inv[0][*], row1 to inv[1][*], etc.
 109     *
 110     * We *didn't* do the swapping before the elimination so that we could more
 111     * easily keep track of what ops are needed to be done in the inverse.
 112     */
 113    MAT3_inv3_swap(inv, row0, row1, row2);
 114
 115    return(TRUE);
 116 }
 117
 118 /*
 119  * Fast inversion routine for 3 x 3 matrices.   - Written by jfh.
 120  *
 121  * This takes 30 multiplies/divides, as opposed to 39 for Cramer's Rule.
 122  * The algorithm consists of performing fast gaussian elimination, by never
 123  * doing any operations where the result is guaranteed to be zero, or where
 124  * one operand is guaranteed to be zero. This is done at the cost of clarity,
 125  * alas.
 126  *
 127  * Returns 1 if the inverse was successful, 0 if it failed.
 128  */
 129
 130 static int
 131 MAT3_invert3 (register double source[3][3], register double inv[3][3])
 132 {
 133    register int i, row0;
 134    double       temp;
 135    double       a, b, c;
 136
 137    inv[0][0] = inv[1][1] = inv[2][2] = 1.0;
 138    inv[0][1] = inv[0][2] = inv[1][0] = inv[1][2] = inv[2][0] = inv[2][1] = 0.0;
 139
 140    /* attempt to find the largest entry in first column to use as pivot */
 141    a = source[0][0]; if (a < 0) a = -a;
 142    b = source[1][0]; if (b < 0) b = -b;
 143    c = source[2][0]; if (c < 0) c = -c;
 144
 145    if (a > b) {
 146       if (a > c) row0 = 0;
 147       else row0 = 2;
 148    }
 149    else {
 150       if (b > c) row0 = 1;
 151       else row0 = 2;
 152    }
 153
 154    /* Scale row0 of source */
 155    if ((source[row0][0] < SMALL) && (source[row0][0] > -SMALL)) return(FALSE);
 156    temp = 1.0 / source[row0][0];
 157    source[row0][0]  = 1.0;
 158    source[row0][1] *= temp;
 159    source[row0][2] *= temp;
 160
 161    /* Scale row0 of inverse */
 162    inv[row0][row0] = temp;      /* other entries are zero -- no effort  */
 163
 164    /* Clear column zero of source, and make corresponding changes in inverse */
 165
 166    for (i = 0; i < 3; i++) if (i != row0) {     /* for i = all rows but row0 */
 167       temp = -source[i][0];
 168       source[i][0]  = 0.0;
 169       source[i][1] += temp * source[row0][1];
 170       source[i][2] += temp * source[row0][2];
 171       inv[i][row0]  = temp * inv[row0][row0];
 172    }
 173
 174    /*
 175     * We've now done gaussian elimination so that the source and
 176     * inverse look like this:
 177     *
 178     *   1 * *           * 0 0
 179     *   0 * *           * 1 0
 180     *   0 * *           * 0 1
 181     *
 182     * We now proceed to do elimination on the second column.
 183     */
 184    if (! MAT3_inv3_second_col(source, inv, row0)) return(FALSE);
 185
 186    return(TRUE);
 187 }
 188
 189 /*
 190  * Finds a new pivot for a non-simple 4x4.  See comments in MAT3invert().
 191  */
 192
 193 static int
 194 MAT3_inv4_pivot (register MAT3mat src, MAT3vec r, double *s, int *swap)
 195 {
 196    register int i, j;
 197    double       temp, max;
 198
 199    *swap = -1;
 200
 201    if (MAT3_IS_ZERO(src[3][3])) {
 202
 203       /* Look for a different pivot element: one with largest abs value */
 204       max = 0.0;
 205
 206       for (i = 0; i < 4; i++) {
 207          if      (src[i][3] >  max) max =  src[*swap = i][3];
 208          else if (src[i][3] < -max) max = -src[*swap = i][3];
 209       }
 210
 211       /* No pivot element available ! */
 212       if (*swap < 0) return(FALSE);
 213
 214       else for (j = 0; j < 4; j++) SWAP(src[*swap][j], src[3][j], temp);
 215    }
 216
 217    MAT3_SET_VEC (r, -src[0][3], -src[1][3], -src[2][3]);
 218
 219    *s = 1.0 / src[3][3];
 220
 221    src[0][3] = src[1][3] = src[2][3] = 0.0;
 222    src[3][3]                         = 1.0;
 223
 224    MAT3_SCALE_VEC(src[3], src[3], *s);
 225
 226    for (i = 0; i < 3; i++) {
 227       src[0][i] += r[0] * src[3][i];
 228       src[1][i] += r[1] * src[3][i];
 229       src[2][i] += r[2] * src[3][i];
 230    }
 231
 232    return(TRUE);
 233 }
 234
 235 /* -------------------------  Internal Routines  --------------------------- */
 236
 237 /* --------------------------  Public Routines  ---------------------------- */
 238
 239 /*
 240  * This returns the inverse of the given matrix.  The result matrix
 241  * may be the same as the one to invert.
 242  *
 243  * Fast inversion routine for 4 x 4 matrices, written by jfh.
 244  *
 245  * Returns 1 if the inverse was successful, 0 if it failed.
 246  *
 247  * This routine has been specially tweaked to notice the following:
 248  * If the matrix has the form
 249  *        * * * 0
 250  *        * * * 0
 251  *        * * * 0
 252  *        * * * 1
 253  *
 254  * (as do many matrices in graphics), then we compute the inverse of
 255  * the upper left 3x3 matrix and use this to find the general inverse.
 256  *
 257  *   In the event that the right column is not 0-0-0-1, we do gaussian
 258  * elimination to make it so, then use the 3x3 inverse, and then do
 259  * our gaussian elimination.
 260  */
 261
 262 int
 263 MAT3invert(double (*result_mat)[4], double (*mat)[4])
 264 {
 265    MAT3mat              src, inv;
 266    register int         i, j, simple;
 267    double               m[3][3], inv3[3][3], s, temp;
 268    MAT3vec              r, t;
 269    int                  swap;
 270
 271    MAT3copy(src, mat);
 272    MAT3identity(inv);
 273
 274    /* If last column is not (0,0,0,1), use special code */
 275    simple = (mat[0][3] == 0.0 && mat[1][3] == 0.0 &&
 276              mat[2][3] == 0.0 && mat[3][3] == 1.0);
 277
 278    if (! simple && ! MAT3_inv4_pivot(src, r, &s, &swap)) return(FALSE);
 279
 280    MAT3_COPY_VEC(t, src[3]);    /* Translation vector */
 281
 282    /* Copy upper-left 3x3 matrix */
 283    for (i = 0; i < 3; i++) for (j = 0; j < 3; j++) m[i][j] = src[i][j];
 284
 285    if (! MAT3_invert3(m, inv3)) return(FALSE);
 286
 287    for (i = 0; i < 3; i++) for (j = 0; j < 3; j++) inv[i][j] = inv3[i][j];
 288
 289    for (i = 0; i < 3; i++) for (j = 0; j < 3; j++)
 290       inv[3][i] -= t[j] * inv3[j][i];
 291
 292    if (! simple) {
 293
 294       /* We still have to undo our gaussian elimination from earlier on */
 295       /* add r0 * first col to last col */
 296       /* add r1 * 2nd   col to last col */
 297       /* add r2 * 3rd   col to last col */
 298
 299       for (i = 0; i < 4; i++) {
 300          inv[i][3] += r[0] * inv[i][0] + r[1] * inv[i][1] + r[2] * inv[i][2];
 301          inv[i][3] *= s;
 302       }
 303
 304       if (swap >= 0)
 305          for (i = 0; i < 4; i++) SWAP(inv[i][swap], inv[i][3], temp);
 306    }
 307
 308    MAT3copy(result_mat, inv);
 309
 310    return(TRUE);
 311 }