FMatrix.cc

Go to the documentation of this file.

#include "FMatrix.h"
 
 
FMatrix::~FMatrix()
{
  for ( int i = 0; i < mxNRows; i++ )
    free( Ent[i] );
  free(Ent);
  if (decomposed) {
    free(base1);
    free(++(indx));
  }
}
 
void
FMatrix:: Zero()
{
  for (int i = 0; i < mxNRows; i++ )
    memset( Ent[i], 0, mxNCols * sizeof(Real) );
  decomposed = false;
}
 
 
FMatrix&
FMatrix::operator=(FMatrix &A)
{
  // make sure B is large enough to accomodate A
  if ((mxNRows < A.NRows) || (mxNCols < A.NCols))
    A.resize(NRows, NCols);
 
  A.NCols = NCols;
  A.NRows = NRows;
  A.lsize = lsize;
 
  for (int i = 0; i < NRows; i++)
    memcpy(Ent[i], A.Ent[i], sizeof(Ent[i][0])*NCols);
  return *this;
}
 
 
void
FMatrix::resize( int nr, int nc )
{
  if ( nr > mxNRows || nc > mxNCols )
  {
    Ent = (Real **)realloc(Ent,nr*sizeof(Real*));
    for( int i=0; i<nc; i++ )
      Ent[i] = (Real*)realloc(Ent[i], nc*sizeof(Real));
  }
  NRows = nr;
  NCols = nc;
  if( decomposed ) {
    free( base1 );
    free( ++indx );
    base1 = NULL;
    indx = NULL;
  }
  Zero();
}
 
 
void FM_Times_FM(FMatrix *A, FMatrix *B, FMatrix *C)
{
  if (A->NCols != B->NRows || C->NRows != A->NRows || C-> NCols != B->NCols ||
      !A->NCols || !A->NRows || !B->NCols)
  {
    fprintf(stderr, "\nDimension compatibility error in %s()\n", __func__);
    fprintf(stderr, "A->NRows=%d; A->NCols=%d\n"
            "B->NRows=%d; B->NCols=%d\n"
            "C->NRows=%d; C->NCols=%d\n\n",
            A->NRows, A->NCols, B->NRows, B->NCols, C->NRows, C->NCols);
    exit (1);
  }
 
  for (int i = 0; i < A->NRows; i++)
  {
    for (int j = 0; j < B->NCols; j++)
    {
      C->Ent[i][j] = A->Ent[i][0] * B->Ent[0][j];
      for (int k = 1; k < A->NCols; k++)
      {
        C->Ent[i][j] += A->Ent[i][k] * B->Ent[k][j];
      }
    }
  }
}
 
void FM_Times_FMinArray(FMatrix *A, FMatrix *B, Real *C)
{
  if (A->NCols != B->NRows || !A->NCols || !A->NRows || !B->NCols)
  {
    fprintf(stderr, "\nDimension compatibility error in %s()\n", __func__);
    fprintf(stderr, "A->NRows=%d; A->NCols=%d\n"
            "B->NRows=%d; B->NCols=%d\n",
            A->NRows, A->NCols, B->NRows, B->NCols);
    exit (1);
  }
 
  for (int i = 0; i < A->NRows; i++)
  {
    for (int j = 0; j < B->NCols; j++)
    {
      C[A->NRows * i + j] = A->Ent[i][0] * B->Ent[0][j];
      for (int k = 1; k < A->NCols; k++)
      {
        C[A->NRows * i + j] += A->Ent[i][k] * B->Ent[k][j];
      }
    }
  }
}
 
void TransFM_Times_FM(FMatrix *A, FMatrix *B, FMatrix *C)
{
  if (A->NRows != B->NRows || C->NRows != A->NCols || C-> NCols != B->NCols ||
      !A->NRows || !A->NCols || !B->NCols)
  {
    fprintf(stderr, "\nDimension compatibility error in %s()\n", __func__);
    fprintf(stderr, "A->NRows=%d; A->NCols=%d\n"
            "B->NRows=%d; B->NCols=%d\n"
            "C->NRows=%d; C->NCols=%d\n\n",
            A->NCols, A->NRows, B->NRows, B->NCols, C->NRows, C->NCols);
    exit (1);
  }
 
  for (int i = 0; i < A->NCols; i++)
  {
    for (int j = 0; j < B->NCols; j++)
    {
      C->Ent[i][j] = A->Ent[0][i] * B->Ent[0][j];
      for (int k = 1; k < A->NRows; k++)
      {
        C->Ent[i][j] += A->Ent[k][i] * B->Ent[k][j];
      }
    }
  }
}
 
void FM_Times_TransFM(FMatrix *A, FMatrix *B, FMatrix *C)
{
  if (A->NCols != B->NCols || C->NRows != A->NRows || C-> NCols != B->NRows ||
      !A->NCols || !A->NRows || !B->NRows)
  {
    fprintf(stderr, "\nDimension compatibility error in %s()\n", __func__);
    fprintf(stderr, "A->NRows=%d; A->NCols=%d\n"
            "B->NCols=%d; B->NRows=%d\n"
            "C->NRows=%d; C->NCols=%d\n\n",
            A->NRows, A->NCols, B->NCols, B->NRows, C->NRows, C->NCols);
    exit (1);
  }
 
  for (int i = 0; i < A->NRows; i++)
  {
    for (int j = 0; j < B->NRows; j++)
    {
      C->Ent[i][j] = A->Ent[i][0] * B->Ent[j][0];
      for (int k = 1; k < A->NCols; k++)
      {
        C->Ent[i][j] += A->Ent[i][k] * B->Ent[j][k];
      }
    }
  }
}
 
void FM_Plus_FM(FMatrix *A, FMatrix *B, FMatrix *C)
{
  if ( A->NCols != B->NCols || A->NRows != B->NRows || C->NRows != A->NRows || C->NCols != A->NCols)
  {
    fprintf(stderr, "\nDimension compatibility error in %s()\n", __func__);
    fprintf(stderr, "A->NRows=%d; A->NCols=%d\n"
            "B->NRows=%d; B->NCols=%d\n"
            "C->NRows=%d; C->NCols=%d\n\n",
            A->NRows, A->NCols, B->NRows, B->NCols, C->NRows, C->NCols);
    exit (1);
  }
 
  for (int i = 0; i < A->NRows; i++)
  {
    for (int j = 0; j < B->NCols; j++)
    {
      C->Ent[i][j] = A->Ent[i][j] + B->Ent[i][j];
    }
  }
}
 
 
 
void
FM_Times_Scalar(FMatrix *A, Real a)
{
  for (int i = 0; i < A->NRows; i++)
    for (int j = 0; j < A->NCols; j++)
      A->Ent[i][j] *= a;
}
 
Real FMatrix_Det(FMatrix *M)
{
 
  Real det = 0;
 
  if (M->NRows != M->NCols)
  {
    fprintf(stderr, "\nError in %s(); M.NRows != M.NCols\n", __func__ );
    exit(1);
  }
 
  switch (M->NRows)
  {
  case 2:
    det = M->Ent[0][0] * M->Ent[1][1] - M->Ent[0][1] * M->Ent[1][0];
    break;
  case 3:
    for (int i = 0; i < M->NRows; i++)
    {
      Real temp = M->Ent[1][(1 + i) % M->NRows] * M->Ent[2][(2 + i) % M->NRows];
      temp     -= M->Ent[2][(1 + i) % M->NRows] * M->Ent[1][(2 + i) % M->NRows];
      det += temp * M->Ent[0][i];
    }
    break;
  case 4:
  {
    FMatrix tempM(M->NRows - 1, M->NCols - 1);
    for (int i = 0; i < M->NRows; i++)
    {
      int j, m;
      for ( j = 0, m = 0; j < M->NRows; j++, m++)
      {
        if (j == i) j++;
        int n = 0;
        for (int k = 1; k < M->NCols; k++, n++)
        {
          if (m < tempM.NRows)
          {
            tempM.Ent[m][n] = M->Ent[j][k];
          }
        }
      }
      det += (i % 2 ? -1.0 : 1.0) * M->Ent[i][0] * FMatrix_Det(&tempM);
    }
  }
  break;
  default:
    fprintf(stderr, "\nError in Det routine; M.NRows must = 2,3, or 4...\n"
            "The 4 case should be general enough for all matrices\n"
            "of size>4 but should be checked; M.NRows=%d\n", M->NRows);
    exit (1);
  }
  return det;
}
 
 
Real FMatrix_Trace(FMatrix *M)
{
 
  Real trace = 0;
 
  if (M->NRows != M->NCols)
  {
    fprintf(stderr, "\nError in %s(); M.NRows != M.NCols\n", __func__ );
    exit(1);
  }
 
  for (int i = 0; i < M->NRows; i++)
    trace += M->Ent[i][i];
 
  return trace;
}
 
 
void 
FMatrix::LUD()
{
  if ( NCols != NRows )
  {
    fprintf( stderr, "%s(): Unable to decompose nonsquare matrix\n", __func__ );
    exit(1);
  }
  if ( base1 == NULL )
  {
    base1 = (Real**) malloc((1 + mxNRows) * sizeof(Real*));
    for ( int i = 0; i < mxNRows; i++ )
      base1[i + 1] = Ent[i] - 1;
    indx = (int *)malloc(mxNRows * sizeof(int));
    indx--;
  }
  Real d;
  decomposed = ludcmp( base1, NRows, indx, &d );
  if( !decomposed )
    return;
  for ( int i = 0; i < NRows; i++ )
    Ent[i] = base1[i + 1] + 1; // LU decomposition swaps rows
  decomposed = true;
}
 
 
void 
FMatrix:: LUsolve( Real *rhs )
{
  if ( !decomposed )
  {
    fprintf( stderr, "%s(): Trying to solve undecomposed matrix\n", __func__ );
    exit(1);
  }
  lubksb( base1 , NRows, indx, rhs - 1 );
}
 
 
FMatrix *
FMatrix::Inv( )
{
  FMatrix *A_inv = new FMatrix( NRows, NRows );
 
  Real  *soln = (Real *)malloc(sizeof(Real) * NRows);
 
  LUD();
  for ( int i = 0; i <NRows; i++ )
  {
    for ( int j = 0; j < NRows; j++ )
      soln[j] = j == i;
    LUsolve( soln );
    for ( int j = 0; j < NRows; j++ )
      A_inv->Ent[j][i] = soln[j];
  }
  free(soln);
 
  return A_inv;
}
 
 
FMatrix *
FMatrix::Invert()
{
  // Buffer original matrix A
  FMatrix A_(mxNRows, mxNCols);
  A_ = *this;
  FMatrix *A_inv = new FMatrix( NRows, NRows );
 
  Real     *soln = (Real *)malloc(sizeof(Real) * NRows);
 
  A_.LUD( );
  for ( int i = 0; i < A_.NRows; i++ )
  {
    for ( int j = 0; j < A_.NRows; j++ )
      soln[j] = j == i;
    A_.LUsolve( soln );
    for ( int j = 0; j < A_.NRows; j++ )
      A_inv->Ent[j][i] = soln[j];
  }
 
  free(soln);
 
  return A_inv;
}
 
 
FMatrix *
FMatrix::Transpose(bool ip)
{
  FMatrix *A_t;
  if (!ip )
  {
    A_t = new FMatrix( NCols, NRows );
    for ( int i = 0; i < NRows; i++ )
      for ( int j = 0; j < NCols; j++ )
        A_t->Ent[j][i] = Ent[i][j];
    return A_t;
  }
  else
  {
    // in-place
    for ( int i = 0; i < NRows; i++ )
    {
      for ( int j = i + 1; j < NRows; j++ )
      {
        Real tmp  = Ent[j][i];
        Ent[j][i] = Ent[i][j];
        Ent[i][j] = tmp;
      }
    }
    return this;
  }
}
 
FMatrix *FM_Rotate(FMatrix *R, double angle, int ax)
{
  double sin_th = sin(angle);
  double cos_th = cos(angle);
 
  static const int p[3][2] = { {1, 2},
    {0, 2},
    {0, 1}
  };
  const int *a = p[ax];
  R->Zero();
 
 
  R->Ent[a[0]][a[0]] =  cos_th;
  R->Ent[a[0]][a[1]] =  sin_th;
  R->Ent[a[1]][a[0]] = -sin_th;
  R->Ent[a[1]][a[1]] =  cos_th;
  R->Ent[ax][ax]     = 1.;
 
  return R;
}
 
Real* FM_X_Vec( FMatrix *A, Real *b, Real *c )
{
  if ( !c ) c = (Real*) calloc(A->NRows, sizeof(Real));
 
  for ( int i = 0; i < A->NRows; i++ )
  {
    c[i] = A->Ent[i][0] * b[0];
    for ( int j = 1; j < A->NCols; j++ )
      c[i] += A->Ent[i][j] * b[j];
  }
  return c;
}
 
 
void FM_Times_Vec(Real dx, FMatrix *B, Real*C, Real *result)
{
  const int BRows  = B->NRows;
  const int BCols  = B->NCols;
  if (BCols == 6)
  {
 
    // read C in register variables since it will stay constant for the whole computation
    Real c1 = C[0],  c2 = C[1],  c3 = C[2],  c4 = C[3],  c5 = C[4],  c6 = C[5];
 
    for (int k = 0; k < BRows; k++)
      result[k] += (c1 * B->Ent[k][0] + c2 * B->Ent[k][1] + c3 * B->Ent[k][2] + c4 * B->Ent[k][3] + c5 * B->Ent[k][4] + c6 * B->Ent[k][5]) * dx;
  }
  else   // general algorithm
  {
    for (int k = 0; k < BRows; k++)
      for (int j = 0; j < BCols; j++)
        result[k] += B->Ent[k][j] * C[j] * dx;
  }
}
 
void B_C_BTrans(Real dx, FMatrix *B, Real *C, FMatrix *result)
{
  const int BRows  = B->NRows;
  const int BCols  = B->NCols;
 
  const int RRows  = result->NRows;
  const int RCols  = result->NCols;
 
  // C and result have to be quadratic
  if ( RRows != RCols || RRows != BRows || !BRows || !BCols)
  {
    fprintf(stderr, "\nDimension compatibility error in %s()\n", __func__);
    fprintf(stderr, "B->NRows=%d; B->NCols=%d\n"
            "result->NRows=%d; result->NCols=%d\n\n",
            BRows, BCols, RRows, RCols);
    exit (1);
  }
 
  if (BCols == 6)
  {
 
    // read C in register variables since it will stay constant for the whole computation
    Real c11 = C[0],  c12 = C[1],  c13 = C[2],  c14 = C[3],  c15 = C[4],  c16 = C[5];
    Real c21 = C[6],  c22 = C[7],  c23 = C[8],  c24 = C[9],  c25 = C[10], c26 = C[11];
    Real c31 = C[12], c32 = C[13], c33 = C[14], c34 = C[15], c35 = C[16], c36 = C[17];
    Real c41 = C[18], c42 = C[19], c43 = C[20], c44 = C[21], c45 = C[22], c46 = C[23];
    Real c51 = C[24], c52 = C[25], c53 = C[26], c54 = C[27], c55 = C[28], c56 = C[29];
    Real c61 = C[30], c62 = C[31], c63 = C[32], c64 = C[33], c65 = C[34], c66 = C[35];
 
 
    for (int i = 0; i < BRows; i++)
    {
 
      // read a row of B
      Real b1 = B->Ent[i][0], b2 = B->Ent[i][1], b3 = B->Ent[i][2], b4 = B->Ent[i][3], b5 = B->Ent[i][4], b6 = B->Ent[i][5];
 
      // compute C times BT_col (== B_row)
      Real v1 = c11 * b1 + c21 * b2 + c31 * b3 + c41 * b4 + c51 * b5 + c61 * b6;
      Real v2 = c12 * b1 + c22 * b2 + c32 * b3 + c42 * b4 + c52 * b5 + c62 * b6;
      Real v3 = c13 * b1 + c23 * b2 + c33 * b3 + c43 * b4 + c53 * b5 + c63 * b6;
      Real v4 = c14 * b1 + c24 * b2 + c34 * b3 + c44 * b4 + c54 * b5 + c64 * b6;
      Real v5 = c15 * b1 + c25 * b2 + c35 * b3 + c45 * b4 + c55 * b5 + c65 * b6;
      Real v6 = c16 * b1 + c26 * b2 + c36 * b3 + c46 * b4 + c56 * b5 + c66 * b6;
 
      // compute R_col = B_row * v (= C times BT_col)
      for (int k = 0; k < BRows; k++)
        result->Ent[i][k] += (v1 * B->Ent[k][0] + v2 * B->Ent[k][1] + v3 * B->Ent[k][2] + v4 * B->Ent[k][3] + v5 * B->Ent[k][4] + v6 * B->Ent[k][5]) * dx;
 
    }
 
  }
  else   // general algorithm
  {
    Real * tmp = (Real*)calloc(BCols, sizeof(Real));
    Real scalar_temp;
 
    for (int i = 0; i < BRows; i++)
    {
 
      memset(tmp, 0., BCols * sizeof(Real));
 
      // compute B_row C
      for (int j1 = 0; j1 < BCols; j1++)
        for (int j2 = 0; j2 < BCols; j2++)
          tmp[j2] += B->Ent[i][j1] * C[BCols * j1 + j2];
 
      // compute (B_row C) BT_col
      for (int k = 0; k < BRows; k++)
      {
        scalar_temp = 0.;
        for (int j = 0; j < BCols; j++)
          scalar_temp += tmp[j] * B->Ent[k][j];
 
        result->Ent[i][k] += scalar_temp * dx;
      }
    }
    free(tmp);
  }
}
 
 
void Print_FMatrix(FMatrix *A)
{
  for (int i = 0; i < A->NRows; i++)
  {
    for ( int j = 0; j < A->NCols; j++)
      printf( "%12.8f\t", A->Ent[i][j] );
    printf("\n");
  }
  printf("\n");
}