matrix.cpp

qrdrls算法

/* 
 * FILE --- matrix.cc
 *
 * PURPOSE --- Source file for a class for implementing a matrix of complexs
 * together with matrix operations such as multiplication, transpose, inverse,
 * etc. It is intended to be used with the control library.
 */

// Include Files
#include <math.h>
#include "matrix.h"


// Define this macro if you want to use this utility without linking
// to the RCS Library
#define NO_RCS
#ifdef NO_RCS
#include <stdio.h>            /* printf function */
#else
#include "rcs_prnt.h"          /* rcs_print_ functions */
#endif

/*************************************************************/

MATRIX::MATRIX(int _row  , int _col , complex2 _val )
{
  // No error at start
  matrix_err = MATRIX_NO_ERROR;
  row = _row;
  col = _col;
  if (row < 1 || col < 1)
    {
      matrix_err = INVALID_DIMENTION;
#ifdef NO_RCS
      printf("MATRIX: ERROR --- %d!!!\n", matrix_err);
      printf("Invalid dimension. Cant create matrix ...\n");
#else
      rcs_print("MATRIX: ERROR --- %d!!!\n", matrix_err);
      rcs_print("Invalid dimension. Cant create matrix ...\n");
#endif
    }
  else
    {
      allocate_memory();
    }
  
  // Set the elements
  set_matrix(_val);
}

MATRIX::MATRIX(int _row, int _col, complex2 **_elements)
{
  // No error at start
  matrix_err = MATRIX_NO_ERROR;
  row = _row;
  col = _col;
  if (row < 1 || col < 1)
    {
      matrix_err = INVALID_DIMENTION;
#ifdef NO_RCS
      printf("MATRIX: ERROR --- %d!!!\n", matrix_err);
      printf("Invalid dimension. Cant create matrix ...\n");
#else
      rcs_print("MATRIX: ERROR --- %d!!!\n", matrix_err);
      rcs_print("Invalid dimension. Cant create matrix ...\n");
#endif
    }
  else
    {
      allocate_memory();
    }
  
  // Set the elements
  if (_elements == NULL)
    {
      set_matrix(complex2(0));
    }
  else
    {
      set_matrix((const complex2 **)_elements);
    }
}

MATRIX::MATRIX(const MATRIX &_matrix)
{
  // No error at start
  matrix_err = MATRIX_NO_ERROR;
  row = _matrix.row;
  col = _matrix.col;
  allocate_memory();
  
  // Set the elements
  set_matrix((const complex2 **)_matrix.elements);
}

MATRIX::~MATRIX()
{
 // free_memory();
}

void MATRIX::free_memory(void)
{
  for (int ii = 0; ii < row; ii++)
    {
//	  if(elements[ii]!=NULL)
      delete elements[ii];
    }
  delete[] elements;
}


void MATRIX::allocate_memory(void)
{
  if ((elements = new complex2*[row]) == NULL)
    {
      matrix_err = MEMORY_ERROR;
#ifdef NO_RCS
      printf("MATRIX: ERROR --- %d!!!\n", matrix_err);
      printf("Not Enought Memory. Cant create matrix ...\n");
#else
      rcs_print("MATRIX: ERROR --- %d!!!\n", matrix_err);
      rcs_print("Not Enought Memory. Cant create matrix ...\n");
#endif
    }
  for (int ii = 0; ii < row; ii++)
    {
      if ((elements[ii] = new complex2[col]) == NULL)
	{
	  matrix_err = MEMORY_ERROR;
#ifdef NO_RCS
	  printf("MATRIX: ERROR --- %d!!!\n", matrix_err);
	  printf("Not Enought Memory. Cant create matrix ...\n");
#else
	  rcs_print("MATRIX: ERROR --- %d!!!\n", matrix_err);
	  rcs_print("Not Enought Memory. Cant create matrix ...\n");
#endif
	}
    }
}

void MATRIX::set_matrix(const complex2 _val)
{
  int ii, jj; // Index variales for the loops
  
  for (ii = 0; ii < row; ii++)                                                                                                                                                                                                                                           
    {
      for (jj = 0; jj < col; jj++)
	{
	  elements[ii][jj] = _val;
	}
    }
}

void MATRIX::set_matrix(const complex2 **_elements)
{
  int ii, jj; // Index variales for the loops
  
  for (ii = 0; ii < row; ii++)
    {
      for (jj = 0; jj < col; jj++)
	{
	  elements[ii][jj] = _elements[ii][jj];
	}
    }
}

void MATRIX::set_matrix(const complex2 *_elements)
{
  if (col == 1)
    {
      set_col(0, _elements);
    }
  else if (row == 1)
    {
      set_row(0, _elements);
    }
  else
    {
      matrix_err = INVALID_SET_MATRIX;
#ifdef NO_RCS
      printf("MATRIX:set_matrix: ERROR --- %d!!!\n", matrix_err);
      printf("Matrix not vector. Provide two dimetional array ...\n");
#else
      rcs_print("MATRIX:set_matrix: ERROR --- %d!!!\n", matrix_err);
      rcs_print("Matrix not vector. Provide two dimetional array ...\n");
#endif

    }
}


void MATRIX::set_diagonal(complex2 _val)
{
  if (row != col)
    {
      // Find the smaller of # of rows or columns
      //      int s = (row < col) ? row : col;
      //      for (int ii = 0; ii < s; ii++)
      //	{
      //	  elements[ii][ii] = _val;
      //	}
      matrix_err = NONSQUARE_MATRIX;
#ifdef NO_RCS
      printf("MATRIX:set_diagonal: ERROR --- %d!!!\n", matrix_err);
      printf("Nonsqure matrix. Cant set the diagonal ...\n");
#else
      rcs_print("MATRIX:set_diagonal: ERROR --- %d!!!\n", matrix_err);
      rcs_print("Nonsqure matrix. Cant set the diagonal ...\n");
#endif
    }
  else
    {
      for (int ii = 0; ii < row; ii++)
	{
	  elements[ii][ii] = _val;
	}
    }
}

void MATRIX::set_diagonal(const complex2 *_elements)
{
  if (row != col)
    {
      // Find the smaller of # of rows or columns
      //  int s = (row < col) ? row : col;
      //  for (int ii = 0; ii < s; ii++)
      //    {
      //      elements[ii][ii] = _elements[ii];
      //    }
      matrix_err = NONSQUARE_MATRIX;
#ifdef NO_RCS
      printf("MATRIX:set_diagonal: ERROR --- %d!!!\n", matrix_err);
      printf("Nonsqure matrix. Cant set the diagonal ...\n");
#else
      rcs_print("MATRIX:set_diagonal: ERROR --- %d!!!\n", matrix_err);
      rcs_print("Nonsqure matrix. Cant set the diagonal ...\n");
#endif
    }
  else
    {
      for (int ii = 0; ii < row; ii++)
	{
	  elements[ii][ii] = _elements[ii];
	}
    }
}

void MATRIX::set_row(int _row, complex2 _val)
{
  if (_row < 0 || _row >= row)
    {
      matrix_err = ILLEGAL_INDEX;
#ifdef NO_RCS
      printf("MATRIX:set_row: ERROR --- %d!!!\n", matrix_err);
      printf("Invalid row index. Cant set row ...\n");
#else
      rcs_print("MATRIX:set_row: ERROR --- %d!!!\n", matrix_err);
      rcs_print("Invalid row index. Cant set row  ...\n");
#endif
    }
  else
    {
      for (int ii = 0; ii < col; ii++)
	{
	  elements[_row][ii] = _val;
	}
    }
}

void MATRIX::set_row(int _row, const complex2 *_elements)
{
  if (_row < 0 || _row >= row)
    {
      matrix_err = ILLEGAL_INDEX;
#ifdef NO_RCS
      printf("MATRIX:set_row: ERROR --- %d!!!\n", matrix_err);
      printf("Invalid row index. Cant set row ...\n");
#else
      rcs_print("MATRIX:set_row: ERROR --- %d!!!\n", matrix_err);
      rcs_print("Invalid row index. Cant set row  ...\n");
#endif
    }
  else
    {
      for (int ii = 0; ii < col; ii++)
	{
	  elements[_row][ii] = _elements[ii];
	}
    }
}

void MATRIX::set_col(int _col, complex2 _val)
{
  if (_col < 0 || _col >= col)
    {
      matrix_err = ILLEGAL_INDEX;
#ifdef NO_RCS
      printf("MATRIX:set_col: ERROR --- %d!!!\n", matrix_err);
      printf("Invalid column index. Cant set column ...\n");
#else
      rcs_print("MATRIX:set_col: ERROR --- %d!!!\n", matrix_err);
      rcs_print("Invalid column index. Cant set column  ...\n");
#endif
    }
  else
    {
      for (int ii = 0; ii < row; ii++)
	{
	  elements[ii][_col] = _val;
	}
    }
}

void MATRIX::set_col(int _col, const complex2 *_elements)
{
  if (_col < 0 || _col >= col)
    {
      matrix_err = ILLEGAL_INDEX;
#ifdef NO_RCS
      printf("MATRIX:set_col: ERROR --- %d!!!\n", matrix_err);
      printf("Invalid column index. Cant set column ...\n");
#else
      rcs_print("MATRIX:set_col: ERROR --- %d!!!\n", matrix_err);
      rcs_print("Invalid column index. Cant set column  ...\n");
#endif
    }
  else
    {
      for (int ii = 0; ii < row; ii++)
	{
	  elements[ii][_col] = _elements[ii];
	}
    }
}

complex2 MATRIX::get_element(int _row, int _col) const
{
  if(_row < 0 || _row >= row || _col < 0 || _col >= col)
    {

#ifdef NO_RCS
      printf("MATRIX:get_element: ERROR !!!\n");
      printf("Invalid index. 0 returned ...\n");
#else

      rcs_print("MATRIX:get_element: ERROR !!!\n");
      rcs_print("Invalid index. 0 returned ...\n");
#endif
      return (0);
    }
  else
    {
       return (elements[_row][_col]);
    }
}


void MATRIX::set_element(int _row, int _col, complex2 _val)
{
  if (_row < 0 || _row >= row || _col < 0 || _col >= col)
    {
      matrix_err = ILLEGAL_INDEX;
#ifdef NO_RCS
      printf("MATRIX:set_element: ERROR --- %d!!!\n", matrix_err);
      printf("Invalid index. Cant set element ...\n");
#else
      rcs_print("MATRIX:set_element: ERROR --- %d!!!\n", matrix_err);
      rcs_print("Invalid index. Cant set element ...\n");
#endif
    }
  else
    {
      elements[_row][_col] = _val;
    }
}

void MATRIX::add_to_element(int _row, int _col, complex2 _val)
{
  if (_row < 0 || _row >= row || _col < 0 || _col >= col)
    {
      matrix_err = ILLEGAL_INDEX;
#ifdef NO_RCS
      printf("MATRIX:add_to_element: ERROR --- %d!!!\n", matrix_err);
      printf("Invalid index. Cant add to element ...\n");
#else
      rcs_print("MATRIX:add_to_element: ERROR --- %d!!!\n", matrix_err);
      rcs_print("Invalid index. Cant add to element ...\n");
#endif
    }
  else
    {
      elements[_row][_col] += _val;
    }
}

void MATRIX::subtract_from_element(int _row, int _col, complex2 _val)
{
  if (_row < 0 || _row >= row || _col < 0 || _col >= col)
    {
      matrix_err = ILLEGAL_INDEX;
#ifdef NO_RCS
      printf("MATRIX:subtract_from_element: ERROR --- %d!!!\n", matrix_err);
      printf("Invalid index. Cant subtract from element ...\n");
#else
      rcs_print("MATRIX:subtract_from_element: ERROR --- %d!!!\n", matrix_err);
      rcs_print("Invalid index. Cant subtract from element ...\n");
#endif
    }
  else
    {
      elements[_row][_col] -= _val;
    }
}

void MATRIX::multiply_element(int _row, int _col, complex2 _val)
{
  if (_row < 0 || _row >= row || _col < 0 || _col >= col)
    {
      matrix_err = ILLEGAL_INDEX;
#ifdef NO_RCS
      printf("MATRIX:multiply_element: ERROR --- %d!!!\n", matrix_err);
      printf("Invalid index. Cant multiply element ...\n");
#else
      rcs_print("MATRIX:multiply_element: ERROR --- %d!!!\n", matrix_err);
      rcs_print("Invalid index. Cant multiply element ...\n");
#endif
    }
  else
    {
      elements[_row][_col] *= _val;
    }
}

void MATRIX::divide_element(int _row, int _col, complex2 _val)
{
  if (_row < 0 || _row >= row || _col < 0 || _col >= col)
    {
      matrix_err = ILLEGAL_INDEX;
#ifdef NO_RCS
      printf("MATRIX:divide_element: ERROR --- %d!!!\n", matrix_err);
      printf("Invalid index. Cant divide element ...\n");
#else
      rcs_print("MATRIX:divide_element: ERROR --- %d!!!\n", matrix_err);
      rcs_print("Invalid index. Cant divide element ...\n");
#endif
    }
  else
    {
      if (!_val.IsZero())
	{
	  elements[_row][_col] /= _val;
	}
      else
	{
	  matrix_err = DIVIDE_BY_ZERO;
#ifdef NO_RCS
	  printf("MATRIX:divide_element: ERROR --- %d!!!\n", matrix_err);
	  printf("Divide by 0. Cant divide element ...\n");
#else
	  rcs_print("MATRIX:divide_element: ERROR --- %d!!!\n", matrix_err);
	  rcs_print("Divide by 0. Cant divide element ...\n");
#endif
	}
    }
}

void MATRIX::add_to_row(int _row, complex2 _val)
{
  if (_row < 0 || _row >= row)
    {
      matrix_err = ILLEGAL_INDEX;
#ifdef NO_RCS
      printf("MATRIX:add_to_row: ERROR --- %d!!!\n", matrix_err);
      printf("Invalid row index. Cant add to row ...\n");
#else
      rcs_print("MATRIX:add_to_row: ERROR --- %d!!!\n", matrix_err);
      rcs_print("Invalid row index. Cant add to row  ...\n");
#endif
    }
  else
    {
      for (int ii = 0; ii < col; ii++)
	{
	  elements[_row][ii] += _val;
	}
    }
}

void MATRIX::subtract_from_row(int _row, complex2 _val)
{
  if (_row < 0 || _row >= row)
    {
      matrix_err = ILLEGAL_INDEX;
#ifdef NO_RCS
      printf("MATRIX:subtract_from_row: ERROR --- %d!!!\n", matrix_err);
      printf("Invalid row index. Cant subtract from row ...\n");
#else
      rcs_print("MATRIX:subtract_from_row: ERROR --- %d!!!\n", matrix_err);
      rcs_print("Invalid row index. Cant subtract from row  ...\n");
#endif
    }
  else
    {
      for (int ii = 0; ii < col; ii++)
	{
	  elements[_row][ii] -= _val;
	}
    }
}

void MATRIX::multiply_row(int _row, complex2 _val)
{
  if (_row < 0 || _row >= row)
    {
      matrix_err = ILLEGAL_INDEX;
#ifdef NO_RCS
      printf("MATRIX:multiply_row: ERROR --- %d!!!\n", matrix_err);
      printf("Invalid row index. Cant multiply row ...\n");
#else
      rcs_print("MATRIX:multiply_row: ERROR --- %d!!!\n", matrix_err);
      rcs_print("Invalid row index. Cant multiply row  ...\n");
#endif
    }
  else
    {
      for (int ii = 0; ii < col; ii++)
	{
	  elements[_row][ii] *= _val;
	}
    }
}

void MATRIX::devide_row(int _row, complex2 _val)
{
  if (_row < 0 || _row >= row)
    {
      matrix_err = ILLEGAL_INDEX;
#ifdef NO_RCS
      printf("MATRIX:divide_row: ERROR --- %d!!!\n", matrix_err);
      printf("Invalid row index. Cant divide row ...\n");
#else
      rcs_print("MATRIX:divide_row: ERROR --- %d!!!\n", matrix_err);
      rcs_print("Invalid row index. Cant divide row  ...\n");
#endif
    }
  else
    {
      if (!_val.IsZero())
	{
	  for (int ii = 0; ii < col; ii++)
	    {
	      elements[_row][ii] /= _val;
	    }
	}
      else
	{
	  matrix_err = DIVIDE_BY_ZERO;
#ifdef NO_RCS
	  printf("MATRIX:divide_row: ERROR --- %d!!!\n", matrix_err);
	  printf("Divide by 0. Cant divide row ...\n");
#else
	  rcs_print("MATRIX:divide_row: ERROR --- %d!!!\n", matrix_err);
	  rcs_print("Divide by 0. Cant divide row ...\n");
#endif
	}
    }
}

void MATRIX::add_to_col(int _col, complex2 _val)
{
  if (_col < 0 || _col >= col)
    {
      matrix_err = ILLEGAL_INDEX;
#ifdef NO_RCS
      printf("MATRIX:add_to_col: ERROR --- %d!!!\n", matrix_err);
      printf("Invalid column index. Cant add to column ...\n");
#else
      rcs_print("MATRIX:add_to_col: ERROR --- %d!!!\n", matrix_err);
      rcs_print("Invalid column index. Cant add to column  ...\n");
#endif
    }
  else
    {
      for (int ii = 0; ii < row; ii++)
	{
	  elements[ii][_col] += _val;
	}
    }
}