home *** CD-ROM | disk | FTP | other *** search
/ Language/OS - Multiplatform Resource Library / LANGUAGE OS.iso / a_utils / expanded.lha / expanded / src / Matrix.C < prev    next >
Encoding:
C/C++ Source or Header  |  1992-03-19  |  26.9 KB  |  1,013 lines
  1. //
  2. // Linear-Affine-Projective Geometry Package
  3. //
  4. // Matrix.C
  5. //
  6. // $Header$
  7. //
  8. // Austin Dahl and William J.R. Longabaugh
  9. // University of Washington
  10. //
  11. // Copyright (c) 1990, 1991, 1992 Austin Dahl and William J.R. Longabaugh
  12. //   Copying, use and development for non-commercial purposes permitted.
  13. //   All rights for commercial use reserved.
  14. //   This software is unsupported and without warranty; it is
  15. //   provided "as is".
  16. //
  17. // Original implementation by Austin Dahl.  Modified by WJRL to eliminate
  18. // fixed maximum size and to simplify operations on identity matrices.
  19. // Also modified to use routines based on algorithms described in
  20. // "Numerical Recipes in C" by Press et. al. for matrix inversion
  21. // and determinants.
  22. //
  23. // ***********************************************************************
  24.  
  25. #include <math.h>
  26. #include <malloc.h>
  27. #include <stream.h>
  28. #include "Matrix.h"
  29.  
  30. #define OPENBRACKET_CHAR '{'
  31. #define OPENBRACKET_STRING "{"
  32. #define CLOSEBRACKET_CHAR '}'
  33. #define CLOSEBRACKET_STRING "}"
  34. #define COMMA_CHAR ','
  35. #define SPACE_CHAR ' '
  36. #define NULL_CHAR '\0'
  37.  
  38. // Function prototypes for local functions for doing LU decomposition
  39. // and subsequent matrix inversion:
  40.  
  41. static Boolean lud(Matrix& arg, int perm[], int* swap_par, Matrix* mat);
  42. static void lu2inv(Matrix& mat, int perm[], Matrix* invp);
  43.  
  44. // ***********************************************************************
  45.  
  46. RowMatrix::RowMatrix() {
  47.     columns = 0;
  48.     rmr = NULL;
  49. }
  50.  
  51. // ***********************************************************************
  52.  
  53. RowMatrix::RowMatrix(RowMatrix &m) {
  54.     columns = m.Columns();
  55.     if (columns > MAX_LOCAL_DIMEN) {
  56.       rmr = new Scalar[columns];
  57.     } else {
  58.       rmr = local;
  59.     }
  60.     for (int c = 0; c < columns; c++) {
  61.       rmr[c] = m[c];
  62.     }
  63. }  
  64.  
  65. // ***********************************************************************
  66.  
  67. RowMatrix& RowMatrix::operator=(RowMatrix &m)
  68. {
  69.     if ((rmr != NULL) && (columns > MAX_LOCAL_DIMEN)) {
  70.       delete rmr;
  71.     }
  72.  
  73.     columns = m.Columns();
  74.     if (columns > MAX_LOCAL_DIMEN) {
  75.       rmr = new Scalar[columns];
  76.     } else {
  77.       rmr = local;
  78.     }
  79.  
  80.     for (int c = 0; c < columns; c++) {
  81.       rmr[c] = m[c];
  82.     }
  83.     return (*this);
  84. }  
  85.  
  86. // ***********************************************************************
  87.  
  88. RowMatrix::~RowMatrix()
  89. {
  90.     if ((rmr != NULL) && (columns > MAX_LOCAL_DIMEN)) {
  91.       delete rmr;
  92.     }
  93. }
  94.  
  95. // ***********************************************************************
  96.  
  97. RowMatrix::RowMatrix(int c) {
  98.     if (c < 0)
  99.       errh.ErrorExit("RowMatrix::RowMatrix(int c)",
  100.              "c is negative",
  101.              ErrVal("c = ", c));
  102.     columns = c;
  103.     if (c > MAX_LOCAL_DIMEN) {
  104.       rmr = new Scalar[c];
  105.     } else {
  106.       rmr = local;
  107.     }
  108. }
  109.  
  110. // ***********************************************************************
  111.  
  112. RowMatrix::RowMatrix(Scalar s0) {
  113.     columns = 1;
  114.     rmr = local;
  115.     rmr[0] = s0;
  116. }
  117.  
  118. // ***********************************************************************
  119.  
  120. RowMatrix::RowMatrix(Scalar s0, Scalar s1) {
  121.     columns = 2;
  122.     rmr = local;
  123.     rmr[0] = s0;
  124.     rmr[1] = s1;
  125. }
  126.  
  127. // ***********************************************************************
  128.  
  129. RowMatrix::RowMatrix(Scalar s0, Scalar s1, Scalar s2) {
  130.     columns = 3;
  131.     rmr = local;
  132.     rmr[0] = s0;
  133.     rmr[1] = s1;
  134.     rmr[2] = s2;
  135. }
  136.  
  137. // ***********************************************************************
  138.  
  139. RowMatrix::RowMatrix(Scalar s0, Scalar s1, Scalar s2, Scalar s3) {
  140.     columns = 4;
  141.     rmr = local;
  142.     rmr[0] = s0;
  143.     rmr[1] = s1;
  144.     rmr[2] = s2;
  145.     rmr[3] = s3;
  146. }
  147.  
  148. // ***********************************************************************
  149.  
  150. Scalar& RowMatrix::operator[](int c) {
  151.     if ((c < 0) || (c >= Columns()))
  152.       errh.ErrorExit("Scalar& RowMatrix::operator[](int c)",
  153.              "c out of range",
  154.              ErrVal("c = ", c),
  155.              (*this));
  156.     return rmr[c];
  157. }
  158.  
  159. // ***********************************************************************
  160.  
  161. RowMatrix AdjointRow(RowMatrix& rmat, int comit) {
  162.     int c = rmat.Columns();
  163.     int rc = ((comit >= 0) && (comit < c)) ? c - 1 : c;
  164.     RowMatrix result(c - 1);
  165.     int i = 0, j = 0;
  166.     while (i < c) {
  167.     if (i != comit) {
  168.         result[j] = rmat[i];
  169.         j++;
  170.     }
  171.     i++;
  172.     }
  173.     return result;
  174. }
  175.  
  176. // ***********************************************************************
  177.  
  178. void RowMatrix::debug_out(ostream& os, int indent) {
  179.     char* iblock = new char[indent + 1];
  180.     for(int i = 0;  i < indent; i++)
  181.       *(iblock + i) = SPACE_CHAR;
  182.     *(iblock + i) = NULL_CHAR;
  183.     int c = Columns();
  184.     os << iblock << c << " column RowMatrix\n";
  185.     os << iblock << OPENBRACKET_STRING << " ";
  186.     for(i = 0; i < c; i++)
  187. #ifdef c_plusplus
  188.       os << form("%12.5f", (*this)[i]);          // cfront
  189. #else
  190.       os.form("%12.5f", (*this)[i]);
  191. #endif
  192.     os << " " << CLOSEBRACKET_STRING << "\n";
  193.     delete iblock;
  194. }    
  195.  
  196. // ***********************************************************************
  197.  
  198. ostream& operator<<(ostream& os, RowMatrix& rmat) {
  199.     int c = rmat.Columns();
  200.     os << OPENBRACKET_STRING << " ";
  201.     for(int i = 0; i < c; i++)
  202.       os << "\t" << rmat[i];
  203.     os << " " << CLOSEBRACKET_STRING;
  204.     return os;
  205. }
  206.  
  207. // ***********************************************************************
  208. // ***********************************************************************
  209.  
  210. Matrix::Matrix() {
  211.     is_identity = FALSE;
  212.     rows = 0;
  213.     columns = 0;
  214.     mr = NULL;
  215. }
  216.  
  217. // ***********************************************************************
  218.  
  219. Matrix::Matrix(Matrix &m) {
  220.     Boolean mi = m.Is_Identity();
  221.     rows = m.Rows();
  222.     columns = m.Columns();
  223.     if (rows > MAX_LOCAL_DIMEN) {
  224.       mr = new RowMatrix[rows];
  225.     } else {
  226.       mr = local;
  227.     }
  228.     for(int k = 0; k < rows; k++)
  229.       mr[k] = RowMatrix(columns);
  230.  
  231.     for(int i = 0; i < rows; i++)
  232.       for(int j = 0; j < columns; j++)
  233.     mr[i][j] = m[i][j];
  234.     is_identity = mi;
  235.     if (mi) m.Set_Identity(TRUE);
  236. }  
  237.  
  238. // ***********************************************************************
  239.  
  240. Matrix& Matrix::operator=(Matrix &m) {
  241.  
  242. // If something is already assigned to this matrix, we may need to
  243. // release free store before assigning a new matrix.  If the matrix
  244. // is using free store to hold the row matrices, deleting the 
  245. // row matrices will call the RowMatrix destructor automatically,
  246. // releasing any free store used by the RowMatrices.  If this matrix
  247. // is using local store to hold row matrices, and those row matrices
  248. // are using free store, that free store will either get released
  249. // when the row matrix gets a new assignment, OR when this matrix
  250. // is finally destroyed.
  251.  
  252.     if ((mr != NULL) && (rows > MAX_LOCAL_DIMEN)) {
  253.       delete [rows] mr;
  254.     }
  255.      
  256.     Boolean mi = m.Is_Identity();
  257.     rows = m.Rows();
  258.     columns = m.Columns();
  259.     if (rows > MAX_LOCAL_DIMEN) {
  260.       mr = new RowMatrix[rows];
  261.     } else {
  262.       mr = local;
  263.     }
  264.  
  265.     for(int k = 0; k < rows; k++)
  266.       mr[k] = RowMatrix(columns);
  267.  
  268.     for(int i = 0; i < rows; i++)
  269.       for(int j = 0; j < columns; j++)
  270.     mr[i][j] = m[i][j];
  271.     is_identity = mi;
  272.     if (mi) m.Set_Identity(TRUE);
  273.     return *this;
  274. }
  275.  
  276. // ***********************************************************************
  277.  
  278. Matrix::~Matrix()
  279. {
  280.   if ((mr != NULL) && (rows > MAX_LOCAL_DIMEN)) {
  281.       delete [rows] mr;
  282.   }
  283. }
  284.  
  285. // ***********************************************************************
  286.  
  287. Matrix::Matrix(int r, int c) {
  288.     if ((r < 0) || (c < 0))
  289.       errh.ErrorExit("Matrix::Matrix(int r, int c)",
  290.              "r or c negative",
  291.              ErrVal("r = ", r),
  292.              ErrVal("c = ", c));
  293.  
  294.     is_identity = FALSE;
  295.     rows = r;
  296.     columns = c;
  297.  
  298.     if (rows > MAX_LOCAL_DIMEN) {
  299.       mr = new RowMatrix[r];
  300.     } else {
  301.       mr = local;
  302.     }
  303.     for(int i = 0; i < r; i++)
  304.       mr[i] = RowMatrix(c);
  305. }
  306.  
  307. // ***********************************************************************
  308.  
  309. Matrix::Matrix(int dim) {
  310.     if (dim < 0)
  311.       errh.ErrorExit("Matrix::Matrix(int dim)",
  312.              "dim is negative",
  313.              ErrVal("dim = ", dim));
  314.  
  315.     is_identity = FALSE;
  316.     rows = columns = dim;
  317.     if (rows > MAX_LOCAL_DIMEN) {
  318.       mr = new RowMatrix[rows];
  319.     } else {
  320.       mr = local;
  321.     }
  322.     for(int i = 0; i < dim; i++)
  323.       mr[i] = RowMatrix(dim);
  324. }
  325.  
  326. // ***********************************************************************
  327.  
  328. Matrix::Matrix(RowMatrix& rm0) {
  329.     is_identity = FALSE;
  330.     rows = 1;
  331.     mr = local;
  332.     mr[0] = rm0;
  333.     columns = mr[0].Columns();
  334. }
  335.  
  336. // ***********************************************************************
  337.  
  338. Matrix::Matrix(RowMatrix& rm0, RowMatrix& rm1) {
  339.     is_identity = FALSE;
  340.     rows = 2;
  341.     mr = local;
  342.     mr[0] = rm0;
  343.     mr[1] = rm1;
  344.     columns = mr[0].Columns();
  345.     if (columns != mr[1].Columns()) {
  346.       errh.ErrorExit("Matrix::Matrix(RowMatrix&, RowMatrix&)",
  347.              "Not all row matrices have same column count",
  348.              (*this));
  349.     }
  350. }
  351.  
  352. // ***********************************************************************
  353.  
  354. Matrix::Matrix(RowMatrix& rm0, RowMatrix& rm1, RowMatrix& rm2) {
  355.     is_identity = FALSE;
  356.     rows = 3;
  357.     mr = local;
  358.     mr[0] = rm0;
  359.     mr[1] = rm1;
  360.     mr[2] = rm2;
  361.     columns = mr[0].Columns();
  362.     for(int i = 1; i < rows; i++) {
  363.       if (columns != mr[i].Columns()) {
  364.         errh.ErrorExit("Matrix::Matrix(RowMatrix&, RowMatrix&)",
  365.                "Not all row matrices have same column count",
  366.                (*this));
  367.       }
  368.     }
  369. }
  370.  
  371. // ***********************************************************************
  372. Matrix::Matrix(RowMatrix& rm0, RowMatrix& rm1, RowMatrix& rm2, RowMatrix& rm3)
  373. {
  374.     is_identity = FALSE;
  375.     rows = 4;
  376.     mr = local;
  377.     mr[0] = rm0;
  378.     mr[1] = rm1;
  379.     mr[2] = rm2;
  380.     mr[3] = rm3;
  381.     columns = mr[0].Columns();
  382.     for(int i = 1; i < rows; i++) {
  383.       if (columns != mr[i].Columns()) {
  384.         errh.ErrorExit("Matrix::Matrix(RowMatrix&, RowMatrix&)",
  385.                "Not all row matrices have same column count",
  386.                (*this));
  387.       }
  388.     }
  389. }
  390.  
  391. // ***********************************************************************
  392.  
  393. Matrix ZeroMatrix(int r, int c) {
  394.     Matrix result(r, c);
  395.     for(int i = 0; i < r; i++)
  396.         for(int j = 0; j < c; j++)
  397.             result[i][j] = 0;
  398.     result.Set_Identity(FALSE);
  399.     return result;
  400. }
  401.  
  402. // ***********************************************************************
  403.  
  404. Matrix IdentityMatrix(int dim) {
  405.     Matrix result = ZeroMatrix(dim);
  406.     for(int i = 0; i < dim; i++)
  407.         result[i][i] = 1;
  408.     result.Set_Identity(TRUE);
  409.     return result;
  410. }
  411.  
  412. // ***********************************************************************
  413.  
  414. RowMatrix& Matrix::operator[](int r) {
  415.     if ((r < 0) || (r >= Rows()))
  416.       errh.ErrorExit("RowMatrix& Matrix::operator[](int r)",
  417.              "r out of range",
  418.              ErrVal("r = ", r),
  419.              (*this));
  420. //  Since we are handing out a ref into the matrix, we cannot guarantee
  421. //  it is an identity anymore:
  422.     is_identity = FALSE;
  423.     return mr[r];
  424. }
  425.  
  426. // ***********************************************************************
  427.  
  428. Matrix Adjoint(Matrix& mat, int romit, int comit) {
  429.     int r = mat.Rows();
  430.     int c = mat.Columns();
  431.     int rr = ((romit >= 0) && (romit < r)) ? r - 1 : r;
  432.     int rc = ((comit >= 0) && (comit < c)) ? c - 1 : c;
  433.     Matrix result(rr, rc);
  434.     int i = 0, j = 0;
  435.     while (i < r) {
  436.     if (i != romit) {
  437.         result[j] = AdjointRow(mat[i], comit);
  438.         j++;
  439.     }
  440.     i++;
  441.     }
  442.     result.Set_Identity(FALSE);
  443.     return result;
  444. }
  445.         
  446. // ***********************************************************************
  447.  
  448. Matrix Transpose(Matrix& mat) {
  449.     int r = mat.Columns();
  450.     int c = mat.Rows();
  451.     if (mat.Is_Identity()) {
  452.       return mat;
  453.     }
  454.     Matrix result(r, c);
  455.     result.Set_Identity(FALSE);
  456.     for(int i = 0; i < r; i++)
  457.         for(int j = 0; j < c; j++)
  458.             result[i][j] = mat[j][i];
  459.     return result;
  460. }
  461.  
  462. // ***********************************************************************
  463.  
  464. Matrix operator+(Matrix& mat1, Matrix& mat2) {
  465.     Boolean m1i = mat1.Is_Identity();
  466.     Boolean m2i = mat2.Is_Identity();
  467.     int r1 = mat1.Rows();
  468.     int r2 = mat2.Rows();
  469.     int c1 = mat1.Columns();
  470.     int c2 = mat2.Columns();
  471.     if ((c1 != c2) || (r1 != r2))
  472.       errh.ErrorExit("Matrix operator+(Matrix& mat1, Matrix& mat2)",
  473.              "matrices are not of the same size",
  474.              mat1,
  475.              mat2);
  476.     Matrix result(r1, c1);
  477.     result.Set_Identity(FALSE);
  478.     for(int i = 0; i < r1; i++)
  479.       for(int j = 0; j < c1; j++)
  480.     result[i][j] = mat1[i][j] + mat2[i][j];
  481.     if (m1i) mat1.Set_Identity(TRUE);
  482.     if (m2i) mat2.Set_Identity(TRUE);
  483.     return result;
  484. }
  485.  
  486. // ***********************************************************************
  487.  
  488. Matrix operator-(Matrix& mat1, Matrix& mat2) {
  489.     Boolean m1i = mat1.Is_Identity();
  490.     Boolean m2i = mat2.Is_Identity();
  491.     int r1 = mat1.Rows();
  492.     int r2 = mat2.Rows();
  493.     int c1 = mat1.Columns();
  494.     int c2 = mat2.Columns();
  495.     if ((c1 != c2) || (r1 != r2))
  496.       errh.ErrorExit("Matrix operator-(Matrix& mat1, Matrix& mat2)",
  497.              "matrices are not of the same size",
  498.              mat1,
  499.              mat2);
  500.     Matrix result = Matrix(r1, c1);
  501.     result.Set_Identity(FALSE);
  502.     for(int i = 0; i < r1; i++)
  503.       for(int j = 0; j < c1; j++)
  504.     result[i][j] = mat1[i][j] - mat2[i][j];
  505.     if (m1i) mat1.Set_Identity(TRUE);
  506.     if (m2i) mat2.Set_Identity(TRUE);
  507.     return result;
  508. }
  509.  
  510. // ***********************************************************************
  511.  
  512. Matrix operator-(Matrix& mat) {
  513.     Boolean mi = mat.Is_Identity();
  514.     int r = mat.Rows();
  515.     int c = mat.Columns();
  516.     Matrix result = Matrix(r, c);
  517.     result.Set_Identity(FALSE);
  518.     for(int i = 0; i < r; i++)
  519.         for(int j = 0; j < c; j++)
  520.             result[i][j] = - mat[i][j];
  521.     if (mi) mat.Set_Identity(TRUE);
  522.     return result;
  523. }
  524.  
  525. // ***********************************************************************
  526.  
  527. Matrix operator*(Matrix& mat1, Matrix& mat2) {
  528.     if (mat1.Is_Identity()) {
  529.       return mat2;
  530.     } else if (mat2.Is_Identity()) {
  531.       return mat1;
  532.     }
  533.     int r1 = mat1.Rows();
  534.     int r2 = mat2.Rows();
  535.     int c1 = mat1.Columns();
  536.     int c2 = mat2.Columns();
  537.     if (c1 != r2)
  538.       errh.ErrorExit("Matrix operator*(Matrix& mat1, Matrix& mat2)",
  539.              "mat1.Columns() != mat2.Rows()",
  540.              mat1,
  541.              mat2);
  542.     int rr = r1;
  543.     int rc = c2;
  544.     Matrix result(rr, rc);
  545.     result.Set_Identity(FALSE);
  546.     for(int i = 0; i < rr; i++)
  547.       for(int j = 0; j < rc; j++) {
  548.       result[i][j] = 0.0;
  549.       for(int k = 0; k < c1; k++)
  550.         result[i][j] += mat1[i][k] * mat2[k][j];
  551.       }
  552.     return result;
  553. }
  554.  
  555. // ***********************************************************************
  556.  
  557. Matrix operator*(Matrix& mat, Scalar scal) {
  558.     Boolean mi = mat.Is_Identity();
  559.     int r = mat.Rows();
  560.     int c = mat.Columns();
  561.     Matrix result = Matrix(r, c);
  562.     result.Set_Identity(FALSE);
  563.     for(int i = 0; i < r; i++)
  564.         for(int j = 0; j < c; j++)
  565.             result[i][j] = scal * mat[i][j];
  566.     if (mi) mat.Set_Identity(TRUE);
  567.     return result;
  568. }
  569.  
  570. // ***********************************************************************
  571.  
  572. Matrix operator/(Matrix& mat, Scalar scal) {
  573.     Boolean mi = mat.Is_Identity();
  574.     if (scal == 0.0)
  575.       errh.ErrorExit("Matrix operator/(Matrix& mat, Scalar scal)",
  576.              "Divide by zero",
  577.              ErrVal("scal = ", scal));
  578.     int r = mat.Rows();
  579.     int c = mat.Columns();
  580.     Matrix result(r, c);
  581.     result.Set_Identity(FALSE);
  582.     for(int i = 0; i < r; i++)
  583.         for(int j = 0; j < c; j++)
  584.             result[i][j] = mat[i][j] / scal;
  585.     if (mi) mat.Set_Identity(TRUE);
  586.     return result;
  587. }
  588.  
  589. // ***********************************************************************
  590.  
  591. Matrix& Matrix::operator+=(Matrix& mat) {
  592.     int r = Rows();
  593.     int c = Columns();
  594.     is_identity = FALSE;
  595.     if ((c != mat.Columns()) || (r != mat.Rows()))
  596.       errh.ErrorExit("Matrix& Matrix::operator+=(Matrix& mat)",
  597.              "matrices are not of the same size",
  598.              (*this),
  599.              mat);
  600.     for(int i = 0; i < r; i++)
  601.       for(int j = 0; j < c; j++)
  602.     (*this)[i][j] += mat[i][j];
  603.     return *this;
  604. }
  605.  
  606. // ***********************************************************************
  607.  
  608. Matrix& Matrix::operator-=(Matrix& mat) {
  609.     int r = Rows();
  610.     int c = Columns();
  611.     is_identity = FALSE;
  612.     if ((c != mat.Columns()) || (r != mat.Rows()))
  613.       errh.ErrorExit("Matrix& Matrix::operator-=(Matrix& mat)",
  614.              "matrices are not of the same size",
  615.              (*this),
  616.              mat);
  617.     for(int i = 0; i < r; i++)
  618.       for(int j = 0; j < c; j++)
  619.     (*this)[i][j] -= mat[i][j];
  620.     return *this;
  621. }
  622.  
  623. // ***********************************************************************
  624.  
  625. Matrix& Matrix::operator*=(Matrix& mat) {
  626.     Matrix result;
  627.     if (is_identity) {
  628.       is_identity = mat.Is_Identity();
  629.       result = mat;
  630.     } else if (mat.Is_Identity()) {
  631.       result = *this;
  632.     } else {
  633.       is_identity = FALSE;
  634.       result = *this * mat;          // hard to multiply in place
  635.     }
  636.     int r = rows = result.Rows();
  637.     int c = columns = result.Columns();
  638.     for(int i = 0; i < r; i++)
  639.       for(int j = 0; j < c; j++)
  640.     (*this)[i][j] = result[i][j];
  641.     return *this;
  642. }
  643.  
  644. // ***********************************************************************
  645.  
  646. Matrix& Matrix::operator*=(Scalar scal) {
  647.     int r = Rows();
  648.     int c = Columns();
  649.     is_identity = FALSE;
  650.     for(int i = 0; i < r; i++)
  651.     for(int j = 0; j < c; j++)
  652.         (*this)[i][j] *= scal;
  653.     return *this;
  654. }
  655.  
  656. // ***********************************************************************
  657.  
  658. Matrix& Matrix::operator/=(Scalar scal) {
  659.     if (scal == 0.0)
  660.       errh.ErrorExit("Matrix& Matrix::operator/=(Scalar scal)",
  661.              "divide by zero",
  662.              ErrVal("scal = ", scal));
  663.     int r = Rows();
  664.     int c = Columns();
  665.     is_identity = FALSE;
  666.     for(int i = 0; i < r; i++)
  667.     for(int j = 0; j < c; j++)
  668.         (*this)[i][j] /= scal;
  669.     return *this;
  670. }
  671.  
  672. // ***********************************************************************
  673.  
  674. void Matrix::debug_out(ostream&  os, int indent) {
  675.     char* iblock = new char[indent + 1];
  676.     for(int i = 0;  i < indent; i++)
  677.       *(iblock + i) = SPACE_CHAR;
  678.     *(iblock + i) = NULL_CHAR;
  679.     int r = Rows();
  680.     os << iblock << ast;
  681.     os << iblock << OPENBRACKET_STRING << " " <<
  682.       r << " by " << Columns() << " Matrix\n";
  683.     for(i = 0; i < r; i++)
  684.     (*this)[i].debug_out(os, indent + 3);
  685.     os << iblock << CLOSEBRACKET_STRING << "\n";
  686.     os << iblock << ast;
  687.     delete iblock;
  688. }
  689.  
  690. // ***********************************************************************
  691.  
  692. ostream& operator<<(ostream& os, Matrix& mat) {
  693.     int r = mat.Rows();
  694.     os << OPENBRACKET_STRING << " ";
  695.      for(int i = 0 ; i < (r - 1); i++)
  696.       os << "\t" << mat[i] << "\n";
  697.     os << "\t" << mat[i] << " " << CLOSEBRACKET_STRING;
  698.     return os;
  699. }
  700.  
  701. // ***********************************************************************
  702. //
  703. // New determinant and inverse routines based on algorithms given in
  704. // "Numerical Recipes in C" by Press et. al.
  705. //
  706. // ***********************************************************************
  707.  
  708. Scalar Det(Matrix& mat)
  709. {
  710.   int n = mat.Rows();
  711.   Scalar retval;
  712.  
  713. //
  714. // Check if matrix is square.  Also do any quick kills:
  715. //
  716.  
  717.   if (n != mat.Columns()) {
  718.     errh.ErrorExit("Scalar Det(Matrix& mat)", "Non-square matrix", mat);
  719.   }
  720.   if (mat.Is_Identity()) {
  721.     return (1.0);
  722.   } else if (n == 1) {
  723.     return (mat[0][0]);
  724.   } else if (n == 2) {
  725.     retval = (mat[0][0] * mat[1][1]) - (mat[0][1] * mat[1][0]);
  726.     return (retval);
  727.   }
  728.  
  729. // Need to dynamically build an integer permutation array to be
  730. // used by the decomposition routine.  (Can't use an IntList,
  731. // since they are defined at a higher level in this system):
  732.  
  733.   int *perm = (int *)malloc((unsigned)(n * sizeof(int)));
  734.  
  735. //
  736. // Use the LU decomposition routine.  Equation 2.5.1 of Press et. al.
  737. // says we just have to multiply together the diagonal elements of
  738. // the LU decomposition.  The parity of the swaps (-1 or 1) is
  739. // found in d. If the LU routine deems the matrix to be singular,
  740. // this routine simply returns 0.0:
  741. //
  742.  
  743.   Matrix res;
  744.   int d;
  745.   if (!lud(mat, perm, &d, &res)) {
  746.     retval = 0.0;
  747.   } else {
  748.     retval = (Scalar)d;
  749.     for (int i = 0; i < n; i++) {
  750.       retval *= res[i][i];
  751.     }
  752.   }
  753.   
  754. //
  755. // Free up permutation array and return:
  756. //
  757.  
  758.   free((char *)perm);
  759.   return ((Scalar)retval);
  760. }
  761.  
  762. // ***********************************************************************
  763. //
  764. // User should call determinant routine first to find out if matrix
  765. // is singular.
  766. //
  767.  
  768. Matrix Inverse(Matrix& mat)
  769. {
  770.   int n = mat.Rows();
  771.  
  772. //
  773. // Check if matrix is square.  Also skip inversion if matrix happens
  774. // to be an identity:
  775. //
  776.     if (n != mat.Columns()) {
  777.       errh.ErrorExit("Matrix Inverse(Matrix& mat)", "Non-square matrix", mat);
  778.     }
  779.     if (mat.Is_Identity()) {
  780.       return mat;
  781.     }
  782.  
  783. //
  784. // Need to dynamically build an integer permutation array to be
  785. // used by the decomposition routine.  (Can't use an IntList,
  786. // since they are defined at a higher level in this system):
  787. //
  788.  
  789.   int *perm = (int *)malloc((unsigned)(n * sizeof(int)));
  790.  
  791. //
  792. // Use the LU decomposition routine followed by routine that converts
  793. // this representation into the inverse.  If the LU routine deems 
  794. // the matrix to be singular (if it finds a pivot near zero) this 
  795. // routine will cause an error:
  796. //
  797.  
  798.   Matrix result, ludm;
  799.   int d;
  800.   if (!lud(mat, perm, &d, &ludm)) {
  801.     errh.ErrorExit("Matrix Inverse(Matrix& mat)", "Matrix is singular", mat);
  802.   }
  803.   lu2inv(ludm, perm, &result);
  804.  
  805. //
  806. // Free up permutation array and return:
  807. //
  808.  
  809.   free((char *)perm);
  810.   return result;
  811. }
  812.  
  813. // ***********************************************************************
  814. // 
  815. // This routine does LU decomposition of a matrix using Crout's method
  816. // with partial pivoting.  It is a new implementation of the algorithm
  817. // described in Press. It returns TRUE if successful, fills in the
  818. // parity of the row swaps, fills in the result matrix with its 
  819. // LU decomposition, and fills in a permutation integer array such 
  820. // that perm[i] holds the integer j, which says that to find the ith row 
  821. // of the original LU, swap in the jth row of returned matrix, GIVEN
  822. // that all previous swaps have also been carried out.
  823. //
  824.  
  825. static Boolean lud(Matrix& arg, int perm[], int* swap_par, Matrix* mat)
  826. {
  827.   int size = arg.Rows();
  828.   RowMatrix scales(size);
  829.   RowMatrix temp;
  830.   int row, col, i;
  831.   Scalar max, val;
  832.  
  833. //
  834. // Initialize swap parity, and copy the argument matrix (we do NOT want
  835. // to destroy the original matrix):
  836. //
  837.  
  838.   *swap_par = 1;
  839.   *mat = arg;
  840.  
  841. //
  842. // The first step is to loop over all the rows to find the largest
  843. // element of each row.  This is needed for "implicit pivoting" as
  844. // described by Press: it is used to scale the comparisons that
  845. // are used to find the largest pivot.  If we find a row with
  846. // only (exact) zeros, report the singularity and quit:
  847. //
  848.  
  849.   for (row = 0; row < size; row++) {
  850.     max = 0.0;
  851.     for (col = 0; col < size; col++) {
  852.       val = fabs((*mat)[row][col]);
  853.       if (val > max) {
  854.         max = val;
  855.       }
  856.     }
  857.     if (max == 0.0) {
  858.       return (FALSE);
  859.     } else {
  860.       scales[row] = 1.0 / max;
  861.     }
  862.   }    
  863.  
  864. //
  865. // Now do Crout's method, which loops over the matrix column by column:
  866. //
  867.  
  868.   for (col = 0; col < size; col++) {
  869.     Scalar hold;
  870.  
  871.   // First thing to do is find the guys above the diagonal, using
  872.   // eq. 2.3.12.  Using a Scalar temp to hold results (as done in 
  873.   // Press) cuts down on [] operator calls:
  874.  
  875.     for (row = 0; row < col; row++) {
  876.       hold = (*mat)[row][col];
  877.       for (i = 0; i < row; i++) {
  878.         hold -= (*mat)[row][i] * (*mat)[i][col];
  879.       }
  880.       (*mat)[row][col] = hold;  
  881.     }
  882.  
  883.   // Now find the guys on and below the diagonal, using equation
  884.   // 2.3.13.  As described in Press, wait before dividing through
  885.   // by the pivot, which may change:
  886.  
  887.     Scalar best_yet = 0.0;
  888.     int pivot_candidate = col;
  889.     for (row = col; row < size; row++) {
  890.       hold = (*mat)[row][col];
  891.       for (i = 0; i < col; i++) {
  892.         hold -= (*mat)[row][i] * (*mat)[i][col];
  893.       }
  894.       (*mat)[row][col] = hold;  
  895.  
  896.       // Figure out if this new value is a better candidate for
  897.       // the pivot, and record it if it is.  Uses the measure
  898.       // described by Press to implement implicit pivoting:
  899.  
  900.       hold = fabs(hold) * scales[row];
  901.       if (hold >= best_yet) {
  902.         best_yet = hold;
  903.         pivot_candidate = row;
  904.       }
  905.     }
  906.  
  907.   // Need to record where this row is coming from, even if there
  908.   // is no swap.  If we have found a better pivot, swap the rows,
  909.   // changing the swap parity.  Keep the scale information consistent,
  910.   // though we don't need the scale anymore for the current row:
  911.  
  912.     perm[col] = pivot_candidate;
  913.     if (pivot_candidate != col) {
  914.       temp = (*mat)[col];
  915.       (*mat)[col] = (*mat)[pivot_candidate];
  916.       (*mat)[pivot_candidate] = temp;
  917.       scales[pivot_candidate] = scales[col];
  918.       *swap_par *= -1;
  919.     }
  920.  
  921.   // Now divide the guys >>below<< the diagonal through by the
  922.   // new pivot.  The diagonal element is not divided, since we
  923.   // actually want to use equation 2.3.12 for it.  If a pivot
  924.   // is still very small, report the singularity and quit:
  925.  
  926.     Scalar factor = (*mat)[col][col];
  927.     if (fabs(factor) < EPSILON) {
  928.       return FALSE;
  929.     }
  930.     for (row = (col + 1); row < size; row++) {
  931.       (*mat)[row][col] /= factor;
  932.     }
  933.   }
  934.   return TRUE;
  935. }
  936.  
  937. // ***********************************************************************
  938. // 
  939. // Given an LU decomposition of a matrix and associated permutation
  940. // key, this routine builds the inverse matrix.  It uses the
  941. // forward/backward substitution algorithm described in Press, using
  942. // columns of an identity matrix for right hand sides.  The Matrix
  943. // is an LU decomposition, the integer array is the permutation key, 
  944. // and the Matrix pointer indicates where to return the result.
  945. //
  946.  
  947. static void lu2inv(Matrix& mat, int perm[], Matrix* invp)
  948. {
  949.   int size = mat.Rows();
  950.   int row, col, k;
  951.   Boolean summing;
  952.   Scalar hold;
  953.   int first_nz;
  954.   int location;
  955.   RowMatrix temprm;
  956.  
  957. // 
  958. // Use an identity matrix to provide the right hand sides.  Permute
  959. // the rows based on the permutation key:
  960. //
  961.  
  962.   *invp = IdentityMatrix(size);
  963.   for (row = 0; row < size; row++) {
  964.     location = perm[row];   
  965.     temprm = (*invp)[location];
  966.     (*invp)[location] = (*invp)[row];
  967.     (*invp)[row] = temprm;
  968.   }
  969.  
  970. //
  971. // Loop through the columns of the rhs matrix, building the inverse
  972. // by columns:
  973. //
  974.  
  975.   for (col = 0; col < size; col++) {
  976.  
  977.     // Start with forward substitution (Press eq. 2.3.6).  Use the
  978.     // optimization they describe, i.e. only start the summing when
  979.     // a non-zero value is hit.  Note that division by the diagonal
  980.     // can be skipped because it always happens to be 1.0:
  981.  
  982.     summing = FALSE;
  983.     first_nz = 0;
  984.     for (row = 0; row < size; row++) {
  985.       hold = (*invp)[row][col];
  986.       if (summing) {
  987.         for (k = first_nz; k < row; k++) {
  988.           hold -= mat[row][k] * (*invp)[k][col]; 
  989.         }  
  990.       } else if (hold != 0.0) {
  991.         summing = TRUE;
  992.         first_nz = row;
  993.       }
  994.       (*invp)[row][col] = hold;
  995.     } 
  996.  
  997.     // Now it's time to do back substitution, which just involves 
  998.     // implementing equation 2.3.7.  Again, using a Scalar temp
  999.     // avoids several [] operator calls:
  1000.  
  1001.     for (row = size - 1; row >= 0; row--) {
  1002.       hold = (*invp)[row][col];
  1003.       for (k = row + 1; k < size; k++) {
  1004.         hold -= mat[row][k] * (*invp)[k][col]; 
  1005.       }  
  1006.       (*invp)[row][col] = hold / mat[row][row];
  1007.     } 
  1008.   }
  1009.   return;
  1010. }
  1011.  
  1012. // ***********************************************************************
  1013.