File indexing completed on 2024-05-05 05:46:11

 /***************************************************************************
  *   Copyright (C) 2003-2004 by David Saxton                               *
  *   david@bluehaze.org                                                    *
  *                                                                         *
  *   This program is free software; you can redistribute it and/or modify  *
  *   it under the terms of the GNU General Public License as published by  *
  *   the Free Software Foundation; either version 2 of the License, or     *
  *   (at your option) any later version.                                   *
  ***************************************************************************/
 
 #include "matrix.h"
 
 #include "element.h"
 
 #include <cmath>
 #include <iostream>
 
 Matrix::Matrix(CUI n, CUI m)
     : m_n(n)
 {
     unsigned int size = m_n + m;
 
     m_mat = new QuickMatrix(size);
     m_lu = new QuickMatrix(size);
 
     m_y = new double[size];
     m_inMap = new int[size];
 
     max_k = 0;
 
     for (unsigned int i = 0; i < size; i++)
         m_inMap[i] = i;
 }
 
 Matrix::~Matrix()
 {
     delete m_mat;
     delete m_lu;
     delete[] m_y;
     delete[] m_inMap;
 }
 
 void Matrix::swapRows(CUI a, CUI b)
 {
     if (a == b)
         return;
     m_mat->swapRows(a, b);
 
     const int old = m_inMap[a];
     m_inMap[a] = m_inMap[b];
     m_inMap[b] = old;
 
     max_k = 0;
 }
 
 void Matrix::performLU()
 {
     unsigned int n = m_mat->size_m();
     if (n == 0)
         return;
 
     // Copy the affected segment to LU
     for (uint i = max_k; i < n; i++) {
         for (uint j = max_k; j < n; j++) {
             (*m_lu)[i][j] = (*m_mat)[i][j];
         }
     }
 
     // LU decompose the matrix, and store result back in matrix
     for (uint k = 0; k < n - 1; k++) {
         double *const lu_K_K = &(*m_lu)[k][k];
         unsigned foo = std::max(k, max_k) + 1;
 
         // detect singular matrixes...
         if (std::abs(*lu_K_K) < 1e-10) {
             if (*lu_K_K < 0.)
                 *lu_K_K = -1e-10;
             else
                 *lu_K_K = 1e-10;
         }
         // #############
 
         for (uint i = foo; i < n; i++) {
             (*m_lu)[i][k] /= *lu_K_K;
         }
 
         for (uint i = std::max(k, max_k) + 1; i < n; i++) {
             const double lu_I_K = (*m_lu)[i][k];
             if (std::abs(lu_I_K) > 1e-12) {
                 m_lu->partialSAF(k, i, foo, -lu_I_K);
             }
         }
     }
 
     max_k = n;
 }
 
 void Matrix::fbSub(QuickVector *b)
 {
     unsigned int size = m_mat->size_m();
 
     for (uint i = 0; i < size; i++) {
         m_y[m_inMap[i]] = (*b)[i];
     }
 
     // Forward substitution
     for (uint i = 1; i < size; i++) {
         double sum = 0;
         for (unsigned int j = 0; j < i; j++) {
             sum += (*m_lu)[i][j] * m_y[j];
         }
         m_y[i] -= sum;
     }
 
     // Back substitution
     m_y[size - 1] /= (*m_lu)[size - 1][size - 1];
     for (int i = size - 2; i >= 0; i--) {
         double sum = 0;
         for (uint j = i + 1; j < size; j++) {
             sum += (*m_lu)[i][j] * m_y[j];
         }
         m_y[i] -= sum;
         m_y[i] /= (*m_lu)[i][i];
     }
 
     // I think we don't need to reverse the mapping because we only permute rows, not columns.
     for (uint i = 0; i < size; i++)
         (*b)[i] = m_y[i];
 }
 
 void Matrix::displayLU()
 {
     uint n = m_mat->size_m();
     for (uint _i = 0; _i < n; _i++) {
         uint i = m_inMap[_i];
         //      uint i = _i;
         for (uint j = 0; j < n; j++) {
             if (j > 0 && (*m_lu)[i][j] >= 0)
                 std::cout << "+";
             std::cout << (*m_lu)[i][j] << "(" << j << ")";
         }
         std::cout << std::endl;
     }
     std::cout << "m_inMap:    ";
     for (uint i = 0; i < n; i++) {
         std::cout << i << "->" << m_inMap[i] << "  ";
     }
     std::cout << std::endl;
     /*cout << "m_outMap:   ";
     for ( uint i=0; i<n; i++ )
     {
         cout << i<<"->"<<m_outMap[i]<<"  ";
     }
     cout << endl;*/
 }