home *** CD-ROM | disk | FTP | other *** search
/ RISC DISC 3 / RISC_DISC_3.iso / resources / etexts / gems / gemsi / matrixortho.c < prev    next >
C/C++ Source or Header  |  1992-09-15  |  1KB  |  51 lines
  1. /* 
  2. Matrix Orthogonalization
  3. Eric Raible
  4. from "Graphics Gems", Academic Press, 1990
  5. */
  6.  
  7. /*
  8.  * Reorthogonalize matrix R - that is find an orthogonal matrix that is
  9.  * "close" to R by computing an approximation to the orthogonal matrix
  10.  *
  11.  *           T  -1/2
  12.  *   RC = R(R R)
  13.  *                                 T      -1
  14.  * [RC is orthogonal because (RC) = (RC) ]
  15.  *                                                       -1/2
  16.  * To compute C, we evaluate the Taylor expansion of F(x) = (I + x)
  17.  * (where x = C - I) about x=0.
  18.  * This gives C = I - (1/2)x + (3/8)x^2 - (5/16)x^3 + ...
  19.  */
  20.  
  21. #include "GraphicsGems.h"
  22.  
  23. static float coef[10] =             /* From mathematica */
  24.   { 1, -1/2., 3/8., -5/16., 35/128., -63/256.,
  25.     231/1024., -429/2048., 6435/32768., -12155/65536. };
  26.  
  27. MATRIX_reorthogonalize (R, limit)
  28.      Matrix4 R;
  29. {
  30.   Matrix4 I, Temp, X, X_power, Sum;
  31.   int power;
  32.  
  33.   limit = MAX(limit, 10);
  34.  
  35.   MATRIX_transpose (R, Temp);        /* Rt */
  36.   MATRIX_multiply (Temp, R, Temp);    /* RtR */
  37.   MATRIX_identify (I);
  38.   MATRIX_subtract (Temp, I, X);    /* RtR - I */
  39.   MATRIX_identify (X_power);        /* X^0 */
  40.   MATRIX_identify (Sum);            /* coef[0] * X^0 */
  41.  
  42.   for (power = 1; power < limit; ++power)
  43.     {
  44.       MATRIX_multiply (X_power, X, X_power);
  45.       MATRIX_constant_multiply (coef[power], X_power, Temp);
  46.       MATRIX_add (Sum, Temp, Sum);
  47.     }
  48.  
  49.   MATRIX_multiply (R, Sum, R);
  50. }
  51.