DOFMatrix.cc

#include "DOFMatrix.h"
#include <algorithm>
#include <png.h>
#include "QPsiPhi.h"
#include "BasisFunction.h"
#include "Boundary.h"
#include "DOFAdmin.h"
#include "ElInfo.h"
#include "FiniteElemSpace.h"
#include "Mesh.h"
#include "DOFVector.h"
#include "Operator.h"
#include "BoundaryCondition.h"
#include "BoundaryManager.h"
#include "ElementMatrix.h"
#include "Assembler.h"

namespace AMDiS {

  DOFMatrix *DOFMatrix::traversePtr = NULL;

  DOFMatrix::DOFMatrix()
  {
    rowFESpace = NULL;
    colFESpace = NULL;
  }

  DOFMatrix::DOFMatrix(const FiniteElemSpace* rowFESpace_,
		       const FiniteElemSpace* colFESpace_,
		       std::string name_)
    : rowFESpace(rowFESpace_),
      colFESpace(colFESpace_),
      name(name_), 
      coupleMatrix(false)
  {
    TEST_EXIT(rowFESpace)("no rowFESpace\n");
  
    if (!colFESpace) {
      colFESpace = rowFESpace;
    }

    if (rowFESpace && rowFESpace->getAdmin())
      (const_cast<DOFAdmin*>(rowFESpace->getAdmin()))->addDOFIndexed(this);

    boundaryManager = NEW BoundaryManager(rowFESpace_);
    elementMatrix = NEW ElementMatrix(rowFESpace->getBasisFcts()->getNumber(),
				      colFESpace->getBasisFcts()->getNumber());

    applyDBCs.clear();
  }

  DOFMatrix::DOFMatrix(const DOFMatrix& rhs)
    : name(rhs.name + "copy")
  {
    *this = rhs;
    if (rowFESpace && rowFESpace->getAdmin())
      (const_cast<DOFAdmin*>( rowFESpace->getAdmin()))->addDOFIndexed(this);
  }

  DOFMatrix::~DOFMatrix()
  {
    FUNCNAME("DOFMatrix::~DOFMatrix()");

    matrix.clear();

    if (rowFESpace && rowFESpace->getAdmin()) {
      (const_cast<DOFAdmin*>(rowFESpace->getAdmin()))->removeDOFIndexed(this);
    }  

    if (boundaryManager)
      DELETE boundaryManager;

    if (elementMatrix)
      DELETE elementMatrix;
  }

  void DOFMatrix::print() const
  {
    FUNCNAME("DOFMatrix::print()");

    int sizeUsed = rowFESpace->getAdmin()->getUsedSize();

    if (static_cast<int>(matrix.size()) < sizeUsed) {
      WARNING("DOFMatrix not yet initialized\n");
      return;
    }

    for (int i = 0; i < sizeUsed; i++) { 
      DOFMatrix::MatrixRow row = matrix[i];
      MSG("row %3d:",i);
      int rowSize = static_cast<int>( row.size());
      for (int j = 0; j < rowSize; j++) {
	int jcol = row[j].col;
	if (entryUsed(i, j)) {
	  Msg::print(" (%3d,%20.17lf)", jcol, row[j].entry);
	}
      }
      Msg::print("\n");
    }
  }

  void DOFMatrix::printRow(int i) const
  {
    FUNCNAME("DOFMatrix::printRow()");

    int sizeUsed = rowFESpace->getAdmin()->getUsedSize();

    if (static_cast<int>(matrix.size()) < sizeUsed) {
      WARNING("DOFMatrix not yet initialized\n");
      return;
    }

    DOFMatrix::MatrixRow row = matrix[i];
    MSG("row %3d:",i);
    int rowSize = static_cast<int>( row.size());
    for (int j = 0; j < rowSize; j++) {
      int jcol = row[j].col;
      if (entryUsed(i,j)) {
	Msg::print(" (%3d,%20.17lf)", jcol, row[j].entry);
      }
    }
    Msg::print("\n");   
  }

  /****************************************************************************/
  /*  clear: remove all entries from dof_matrix                               */
  /****************************************************************************/

  void DOFMatrix::clear()
  {
    int mSize = static_cast<int>(matrix.size());
    for (int i = 0; i < mSize; i++) {
      matrix[i].resize(0);
    }
  }

  bool DOFMatrix::symmetric()
  {
    FUNCNAME("DOFMatrix::symmetric()");

    DegreeOfFreedom row, col;
    double entry, tol = 1e-5;

    DOFMatrix::Iterator matrixRow(this, USED_DOFS);

    for (matrixRow.reset(); !matrixRow.end(); ++matrixRow) {
      row = matrixRow.getDOFIndex();
      int rowSize = matrixRow->size();
      for (int i = 0; i < rowSize; i++) {
	col = (*matrixRow)[i].col;
	entry = (*matrixRow)[i].entry;
	if (abs(entry - logAcc(col, row)) > tol) {
	  MSG("matrix[%d][%d] = %e, matrix[%d][%d] = %e\n",
	      row, col, entry, col, row, logAcc(col, row));

	  return false;
	}
      }
    }

    return true;
  }

  void DOFMatrix::test()
  {
    FUNCNAME("DOFMatrix::test()");

    int non_symmetric = 0, found = 0;

    /* test symmetry */
    for (int i = 0; i < static_cast<int>(matrix.size()); i++) { 
      double sum = 0.0;
      DOFMatrix::MatrixRow *row = &matrix[i];
      for (int j = 0; j < static_cast<int>(row->size()); j++) {
	int jcol = (*row)[j].col;
	if (entryUsed(i, j)) {
	  found = 0;
	  if ((*row)[j].entry != (*row)[j].entry) {
	    MSG("mat[%d,%d]=%10.5e ???\n", i, jcol, (*row)[j].entry);
	    WAIT;
	  }
	  DOFMatrix::MatrixRow *row2 = &matrix[jcol];
	  for (int k = 0; k < static_cast<int>(row->size()); k++) {
	    int kcol = (*row)[k].col;
	    if (entryUsed(jcol, k)) {
	      if (kcol == i) {
		found = 1;
		if (abs((*row2)[k].entry - (*row)[j].entry) > 1.E-5) {
		  non_symmetric = 1;
		  MSG("mat[%d,%d]=%10.5e != mat[%d,%d]=%10.5e\n",
		      i, jcol, (*row)[j].entry, jcol, i, (*row2)[k].entry);
		}
		row2 = NULL;
		break;
	      }
	    }
	  }
	  if (!found) {
	    non_symmetric = 1;
	    MSG("mat[%d,%d] not found\n", jcol, i);
	  }
	}
      }
      if (abs(sum) > 1.E-5) {
	MSG("Zeilensumme[%d] = %10.5e\n", i, sum);
      }
    }

    if (non_symmetric) {
      MSG("matrix `%s' not symmetric.\n", name.data());
    } else {
      MSG("matrix `%s' is symmetric.\n", name.data());
    }
  }


  DOFMatrix& DOFMatrix::operator=(const DOFMatrix& rhs)
  {
    rowFESpace = rhs.rowFESpace;
    colFESpace = rhs.colFESpace;
    operators = rhs.operators;
    operatorFactor = rhs.operatorFactor;
    matrix = rhs.matrix;
    if (rhs.boundaryManager) {
      boundaryManager = NEW BoundaryManager(*rhs.boundaryManager);
    } else {
      boundaryManager = NULL;
    }
    if (rhs.elementMatrix) {
      elementMatrix = NEW ElementMatrix(rowFESpace->getBasisFcts()->getNumber(),
					colFESpace->getBasisFcts()->getNumber());
    } else {
      elementMatrix = NULL;
    }

    return *this;
  }


  void DOFMatrix::addElementMatrix(double sign, 
				   const ElementMatrix &elMat, 
				   const BoundaryType *bound,
				   bool add)
  {
    FUNCNAME("DOFMatrix::addElementMatrix");

    DegreeOfFreedom row;

    int nRow = elMat.rowIndices.getSize();
    int nCol = elMat.colIndices.getSize();

//     elMat.rowIndices.print();
//     elMat.colIndices.print();
//     elMat.print();

    for (int i = 0; i < nRow; i++)  {   // for all rows of element matrix
      row = elMat.rowIndices[i];
      BoundaryCondition *condition = 
	bound ? boundaryManager->getBoundaryCondition(bound[i]) : NULL;

      if (condition && condition->isDirichlet()) {
	applyDBCs.insert(static_cast<int>(row));
      } else {     
	for (int j = 0; j < nCol; j++) {  // for all columns
	  addSparseDOFEntry(sign, row, elMat.colIndices[j], 
			    elMat[i][j], add); 
	}
      }
    }
  }

  double DOFMatrix::logAcc(DegreeOfFreedom a,DegreeOfFreedom b) const
  {
    int j;

    for (j = 0; j < static_cast<int>(matrix[a].size()); j++) 
      if (b == matrix[a][j].col) 
	break;

    return (j == static_cast<int>(matrix[a].size())) ? 0.0 : matrix[a][j].entry;
  }

  void DOFMatrix::changeColOfEntry(DegreeOfFreedom a,
				   DegreeOfFreedom b,
				   DegreeOfFreedom c)
  {
    int j;

    for (j = 0; j<static_cast<int>(matrix[a].size());j++) 
      if (b == matrix[a][j].col) 
	break;

    if (j != static_cast<int>(matrix[a].size())) 
      matrix[a][j].col = c;
  }

  void DOFMatrix::addSparseDOFEntry(double sign, int irow, 
				    int jcol, double entry,
				    bool add)
  {
    FUNCNAME("DOFMatrix::addSparseDOFEntry()");

    MatrixRow *row = &(matrix[irow]);

    if (add && !entry) 
      return;

    int freeCol = -1;
    int rowSize = static_cast<int>(row->size());

    TEST_EXIT_DBG(jcol >= 0 && 
		  jcol < colFESpace->getAdmin()->getUsedSize())
      ("Column index %d out of range 0-%d\n", jcol, colFESpace->getAdmin()->getUsedSize() - 1);

    // first entry is diagonal entry
    if ((rowSize == 0) && (colFESpace == rowFESpace)) {
      MatEntry newEntry = {irow, 0.0};
      row->push_back(newEntry);
      rowSize = 1;
    }

    int i;
    // search jcol
    for (i = 0; i < rowSize; i++) {
      // jcol found ?
      if ((*row)[i].col == jcol) {
	break;
      }
      // remember free entry
      if ((*row)[i].col == UNUSED_ENTRY) {
	freeCol = i;
      }
      // no more entries
      if ((*row)[i].col == NO_MORE_ENTRIES) {
	freeCol = i;
	if (rowSize > i+1) {
	  (*row)[i + 1].entry = NO_MORE_ENTRIES;
	}
	break;
      }
    }

    // jcol found?
    if (i < rowSize) {
      if (!add) {
	(*row)[i].entry = 0.0;
      }

      (*row)[i].entry += sign * entry;
    } else {
      if (freeCol == -1) {
	MatEntry newEntry = {jcol, sign * entry};
	row->push_back(newEntry);
      } else {
	(*row)[freeCol].col = jcol;
	if (!add) 
	  (*row)[freeCol].entry = 0.0;
	(*row)[freeCol].entry += sign * entry;
      }
    } 
  }

  void DOFMatrix::addMatEntry(int row, MatEntry entry)
  {
    matrix[row].push_back(entry);
  }

  void DOFMatrix::addMatEntry(int row,  DegreeOfFreedom col, double value)
  {
    MatEntry entry;
    entry.col = col;
    entry.entry = value;
    matrix[row].push_back(entry);
  }

  void DOFMatrix::addRow(std::vector<MatEntry> row)
  {
    matrix.push_back(row);
  }

  void DOFMatrix::compressDOFIndexed(int first, int last, 
				     std::vector<DegreeOfFreedom> &newDOF)
  {
    for (int i = first; i <= last; i++) {
      if (newDOF[i] >= 0) {
	matrix[newDOF[i]].swap(matrix[i]);
	matrix[i].resize(0);
      }
    }
    int usedSize = rowFESpace->getAdmin()->getUsedSize();
    for (int i = 0; i < usedSize; i++) {
      std::vector<MatEntry> *row = reinterpret_cast<std::vector<MatEntry>*>(&(matrix[i])); 
      int rowSize = static_cast<int>(row->size());
      for (int j = 0; j < rowSize; j++) {
	int col = (*row)[j].col;
	if (entryUsed(i, j)) 
	  (*row)[j].col = newDOF[col];
      }    
    }
  }

  void DOFMatrix::freeDOFContent(int index)
  {
    int col = 0;

    if (0 < matrix[index].size()) {
      int nRows = matrix.size();

      // for all columns in this row
      int size = static_cast<int>(matrix[index].size());
      for (int i = 0; i < size; i++) {
	// if entry is used
	if (entryUsed(index, i)) {
	  // get column of this entry
	  col = matrix[index][i].col;
	  if ((col != index) && (col < nRows)) {  // remove symmetric entry if exists
	    int colsize = static_cast<int>(matrix[col].size());
	    for (int j = 0; j < colsize; j++) {
	      int col2 = matrix[col][j].col;
	      if (col2 == index) {
		matrix[col][j].col = DOFMatrix::UNUSED_ENTRY;
	      } else if (col2 == DOFMatrix::NO_MORE_ENTRIES) {
		break;
	      }
	    }
	  }
	} else if (col == DOFMatrix::NO_MORE_ENTRIES) {
	  break;
	}
      }
      matrix[index].resize(0);	
    }
  }


  void DOFMatrix::assemble(double factor, ElInfo *elInfo, 
			   const BoundaryType *bound, Operator *op)
  {
    FUNCNAME("DOFMatrix::assemble()");

    if (!op && operators.size() == 0) {
      return;
    }

    Operator *operat = op ? op : operators[0];
    operat->getAssembler(omp_get_thread_num())->initElementMatrix(elementMatrix, elInfo);

    if (op) {
      op->getElementMatrix(elInfo, elementMatrix);
    } else {
      std::vector<Operator*>::iterator it;
      std::vector<double*>::iterator factorIt;
      for (it = operators.begin(), factorIt = operatorFactor.begin();	
	   it != operators.end(); 
	   ++it, ++factorIt) {
	if ((*it)->getNeedDualTraverse() == false) {
	  (*it)->getElementMatrix(elInfo, 
				  elementMatrix, 
				  *factorIt ? **factorIt : 1.0);
	}
      }      
    }

    addElementMatrix(factor, *elementMatrix, bound);   
  }

  void DOFMatrix::assemble(double factor, 
			   ElInfo *rowElInfo, ElInfo *colElInfo,
			   ElInfo *smallElInfo, ElInfo *largeElInfo,
			   const BoundaryType *bound, Operator *op)
  {
    FUNCNAME("DOFMatrix::assemble()");

    if (!op && operators.size() == 0) {
      return;
    }

    Operator *operat = op ? op : operators[0];
    operat->getAssembler(omp_get_thread_num())->
      initElementMatrix(elementMatrix, rowElInfo, colElInfo);
    
    if (op) {
      op->getElementMatrix(rowElInfo, colElInfo, 
			   smallElInfo, largeElInfo,
			   elementMatrix);
    } else {
      std::vector<Operator*>::iterator it;
      std::vector<double*>::iterator factorIt;
      for (it = operators.begin(), factorIt = operatorFactor.begin();	
	   it != operators.end(); 
	   ++it, ++factorIt) {
	(*it)->getElementMatrix(rowElInfo, colElInfo,
				smallElInfo, largeElInfo,
				elementMatrix, 
				*factorIt ? **factorIt : 1.0);	
      }      
    }

    addElementMatrix(factor, *elementMatrix, bound);       
  }

  void DOFMatrix::assemble2(double factor, 
			    ElInfo *mainElInfo, ElInfo *auxElInfo,
			    ElInfo *smallElInfo, ElInfo *largeElInfo,			    
			   const BoundaryType *bound, Operator *op)
  {
    FUNCNAME("DOFMatrix::assemble2()");

    if (!op && operators.size() == 0) {
      return;
    }

    Operator *operat = op ? op : operators[0];
    operat->getAssembler(omp_get_thread_num())->
      initElementMatrix(elementMatrix, mainElInfo);
    
    if (op) {
      ERROR_EXIT("TODO");
//       op->getElementMatrix(rowElInfo, colElInfo, 
// 			   smallElInfo, largeElInfo,
// 			   elementMatrix);
    } else {
      std::vector<Operator*>::iterator it;
      std::vector<double*>::iterator factorIt;
      for (it = operators.begin(), factorIt = operatorFactor.begin();	
	   it != operators.end(); 
	   ++it, ++factorIt) {
	if ((*it)->getNeedDualTraverse()) {
	  (*it)->getElementMatrix(mainElInfo, auxElInfo,
				  smallElInfo, largeElInfo,
				  elementMatrix, 
				  *factorIt ? **factorIt : 1.0);
	}
      }      
    }

    addElementMatrix(factor, *elementMatrix, bound);       
  }

  Flag DOFMatrix::getAssembleFlag()
  {
    Flag fillFlag(0);
    std::vector<Operator*>::iterator op;
    for(op = operators.begin(); op != operators.end(); ++op) {
      fillFlag |= (*op)->getFillFlag();
    }

    return fillFlag;
  }

  void DOFMatrix::mm(MatrixTranspose aTranspose,
		     DOFMatrix& a, 
		     MatrixTranspose bTranspose,
		     DOFMatrix& b)
  {
    FUNCNAME("DOFMatrix::mm()");

    WARNING("implementation not finished!!!\n");

    TEST_EXIT_DBG(a.getColFESpace() == b.getRowFESpace())
      ("a.colFESpace != b.rowFESpace\n");
    TEST_EXIT_DBG(rowFESpace == a.getRowFESpace())
      ("rowFESpace != a.rowFESpace\n");  
    TEST_EXIT_DBG(colFESpace == b.getColFESpace())
      ("colFESpace != b.colFESpace\n");  

    clear();

    int i, j;

    if (aTranspose == NoTranspose && bTranspose == NoTranspose) {
      int cols  = b.getColFESpace()->getAdmin()->getUsedSize();
      DOFMatrix::Iterator rowIterator(this, USED_DOFS);

      // for every row ...
      for(rowIterator.reset(); !rowIterator.end(); ++rowIterator) {
	int rowIndex = rowIterator.getDOFIndex();
	// and every column of result   
	for(i=0; i < cols; i++) {
	  double entry = 0.0;
	  // for every entry in a[i] ...
	  for(j=0; j < static_cast<int>( a[rowIndex].size()); j++) {
	    int logIndex = a[rowIndex][j].col;
	    int physIndex = b.logToPhysIndex(logIndex, i);
	    if(physIndex != -1) {
	      entry += a[rowIndex][j].entry * b[logIndex][physIndex].entry;
	    }
	  }
	  if (entry != 0.0) {
	    addSparseDOFEntry(1.0, rowIndex, i, entry);
	  }
	}
      }
    } else if(aTranspose == Transpose && bTranspose == NoTranspose) {
      DOFMatrix::Iterator aIterator(&a, USED_DOFS);
      DOFMatrix::Iterator bIterator(&b, USED_DOFS);
      for(aIterator.reset(), bIterator.reset(); 
	  !aIterator.end(); 
	  ++aIterator, ++bIterator) 
	{
	  std::vector<MatEntry>::const_iterator aRowIt;
	  std::vector<MatEntry>::const_iterator bRowIt;
	  for(aRowIt = aIterator->begin(); aRowIt != aIterator->end(); ++aRowIt) {
	    int aCol = aRowIt->col;
	    if(aCol == UNUSED_ENTRY) continue;
	    if(aCol == NO_MORE_ENTRIES) break;
	    for(bRowIt = bIterator->begin(); bRowIt !=bIterator->end(); ++bRowIt) {
	      int bCol = bRowIt->col;
	      if(bCol == UNUSED_ENTRY) continue;
	      if(bCol == NO_MORE_ENTRIES) break;

	      double entry = aRowIt->entry * bRowIt->entry;

	      if (entry != 0.0) {
		addSparseDOFEntry(1.0, aCol, bCol, entry);	    
	      }
	    }
	  }
	}
    } else if(aTranspose == NoTranspose && bTranspose == Transpose) {
      ERROR_EXIT("not yet\n");
    } else if(aTranspose == Transpose && bTranspose == Transpose) {
      ERROR_EXIT("not yet\n");
    }
  }

  void DOFMatrix::axpy(double a, 
		       const DOFMatrix& x,
		       const DOFMatrix& y)
  {
    FUNCNAME("DOFMatrix::axpy()");

    TEST_EXIT_DBG(x.getRowFESpace() == y.getRowFESpace() &&
		  rowFESpace == x.getRowFESpace())
      ("row fe-spaces not equal\n");
    TEST_EXIT_DBG(x.getColFESpace() == y.getColFESpace() &&
		  colFESpace == x.getColFESpace())
      ("col fe-spaces not equal\n");
    
    DOFMatrix::Iterator rowIterator(this, USED_DOFS);
    DOFMatrix::Iterator xIterator(const_cast<DOFMatrix*>(&x), USED_DOFS);
    DOFMatrix::Iterator yIterator(const_cast<DOFMatrix*>(&y), USED_DOFS);

    int i, rowIndex, colIndex;

    for(rowIterator.reset(), xIterator.reset(), yIterator.reset();
	!rowIterator.end();
	++rowIterator, ++xIterator, ++yIterator)
      {
	rowIndex = rowIterator.getDOFIndex();
	// add x contributions to this row
	for(i=0; i < static_cast<int>((*xIterator).size()); i++) {
	  colIndex = (*xIterator)[i].col;
	  if (colIndex >= 0) {
	    addSparseDOFEntry(a, rowIndex, colIndex, (*xIterator)[i].entry);
	  }
	}
	// add y contributions to this row
	for(i=0; i < static_cast<int>((*yIterator).size()); i++) {
	  colIndex = (*yIterator)[i].col;
	  if (colIndex >= 0) {
	    addSparseDOFEntry(1.0, rowIndex, colIndex, (*yIterator)[i].entry);
	  }
	}
      }
  }

  void DOFMatrix::scal(double b) 
  {
    DOFMatrix::Iterator rowIterator(this, USED_DOFS);
    for (rowIterator.reset(); !rowIterator.end(); ++rowIterator) {
      for (int i = 0; i < static_cast<int>((*rowIterator).size()); i++) {
	if ((*rowIterator)[i].col >= 0) {
	  (*rowIterator)[i].entry *= b;
	}
      }
    }
  }

  void DOFMatrix::addOperator(Operator *op, double* factor, double* estFactor) 
  { 
    operators.push_back(op);
    operatorFactor.push_back(factor);
    operatorEstFactor.push_back(estFactor);
  }

  void DOFMatrix::copy(const DOFMatrix& rhs) 
  {
    clear();
    DOFMatrix::Iterator rhsIterator(const_cast<DOFMatrix*>(&rhs), USED_DOFS);
    DOFMatrix::Iterator thisIterator(this, USED_DOFS);
    std::vector<MatEntry>::const_iterator colIt;
    std::vector<MatEntry>::const_iterator colBegin;
    std::vector<MatEntry>::const_iterator colEnd;
    for(rhsIterator.reset(), thisIterator.reset();
	!rhsIterator.end(); 
	++rhsIterator, ++thisIterator) 
      {
	colBegin = rhsIterator->begin();
	colEnd = rhsIterator->end();
	for(colIt = colBegin; colIt != colEnd; ++colIt) {
	  MatEntry matEntry;
	  matEntry.col = colIt->col;
	  matEntry.entry = colIt->entry;
	  thisIterator->push_back(matEntry);
	}
      }
  }

  void DOFMatrix::removeRowsWithDBC(std::set<int> *rows)
  {
    for (std::set<int>::iterator it = rows->begin();
	 it != rows->end(); 
	 ++it) {
      if (coupleMatrix) {
	matrix[*it].resize(0);
      } else {
	matrix[*it].resize(1);
	matrix[*it][0].col = *it;
	matrix[*it][0].entry = 1.0;
      }
    }

    rows->clear();
  }

  int DOFMatrix::getNumCols() const {
    int max = 0;
    int rowSize = static_cast<int>(matrix.size());
    for (int i = 0; i < rowSize; i++) {
      int colSize = static_cast<int>(matrix[i].size());
      for (int j = 0; j < colSize; j++) {
	if (matrix[i][j].col > max) {
	  max = matrix[i][j].col;
	}
      }
    }
    
    // Add one to the maximum value, because the indeces start with 0.
    return max + 1;
  }


  void DOFMatrix::createPictureFile(const char* filename, int dim)
  {
    png_structp png_ptr = png_create_write_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL);

    if (!png_ptr)
       return;

    png_bytep rowPointers[dim];
    for (int i = 0; i < dim; i++) {
      rowPointers[i] = (png_byte*)png_malloc(png_ptr, dim);

      for (int j = 0; j < dim; j++) {
	rowPointers[i][j] = 255;
      }
    }

    double scalFactor = static_cast<double>(dim) / static_cast<double>(matrix.size());

    for (int i = 0; i < static_cast<int>(matrix.size()); i++) {    
      int pi = static_cast<int>(static_cast<double>(i) * scalFactor);

      TEST_EXIT_DBG((pi >= 0) && (pi < dim))("PI");

      for (int j = 0; j < static_cast<int>(matrix[i].size()); j++) {
	
	int pj = static_cast<int>(static_cast<double>(matrix[i][j].col) * scalFactor);

	TEST_EXIT_DBG((pj >= 0) && (pj < dim))("PJ");

	rowPointers[pi][pj] = 0;
      }
    }
   
    FILE *fp = fopen(filename, "wb");
    TEST_EXIT(fp)("Cannot open file for writing matrix picture file!\n");

    png_infop info_ptr = png_create_info_struct(png_ptr);

    if (!info_ptr) {
       png_destroy_write_struct(&png_ptr, (png_infopp)NULL);
       return;
    }

    png_init_io(png_ptr, fp);

    png_set_IHDR(png_ptr, info_ptr, dim, dim, 8,
		 PNG_COLOR_TYPE_GRAY, 
		 PNG_INTERLACE_NONE,
		 PNG_COMPRESSION_TYPE_DEFAULT,
		 PNG_FILTER_TYPE_DEFAULT);

    png_set_rows(png_ptr, info_ptr, rowPointers);

    png_write_png(png_ptr, info_ptr, PNG_TRANSFORM_IDENTITY, NULL);

    png_destroy_write_struct(&png_ptr, &info_ptr);

    fclose(fp);
  }


  int DOFMatrix::memsize() 
  {   
    int sizeDOFMatrix = sizeof(DOFMatrix);   
    int sizeMatrix = sizeof(std::vector<MatrixRow>);
    for (int i = 0; i < static_cast<int>(matrix.size()); i++) {
      sizeMatrix += sizeof(MatrixRow) + matrix[i].size() * sizeof(MatEntry);
    }
    
    return sizeDOFMatrix + sizeMatrix;
  }


  double norm(std::vector<MatEntry> *row)
  {
    double result = 0.0;
    std::vector<MatEntry>::iterator it;
    for (it = row->begin(); it < row->end(); ++it) {
      result += (*it).entry * (*it).entry;
    }

    return(sqrt(result));
  }

  double min(std::vector<MatEntry> *row)
  {
    double result = 0.0;
    if (row->size() > 0) {
      result = (*row)[0].entry;
    }

    std::vector<MatEntry>::iterator it;
    for (it = row->begin(); it < row->end(); ++it) {
      if ((*it).entry < result) {
	result = (*it).entry;
      }
    }

    return(result);
  }

  double max(std::vector<MatEntry> *row)
  {
    double result = 0.0;
    if (row->size() > 0) {
      result = (*row)[0].entry;
    }

    std::vector<MatEntry>::iterator it;
    for (it = row->begin(); it < row->end(); ++it) {
      if ((*it).entry > result) {
	result = (*it).entry;
      }
    }

    return(result);
  }

  void addDOFMatrix(DOFMatrix *result, const DOFMatrix *a, const DOFMatrix *b)
  {
    result->clear();

    DOFMatrix::Iterator resultIterator(result, USED_DOFS);
    DOFMatrix::Iterator aIterator(const_cast<DOFMatrix*>(a), USED_DOFS);
    DOFMatrix::Iterator bIterator(const_cast<DOFMatrix*>(b), USED_DOFS);

    for (resultIterator.reset(), aIterator.reset(); 
	 !aIterator.end(); 
	 ++resultIterator, ++aIterator) {

      *resultIterator = *aIterator;
    }      

    for (resultIterator.reset(), bIterator.reset(); 
	 !bIterator.end(); 
	 ++resultIterator, ++bIterator) {
      std::vector<MatEntry>::iterator resultRowIt;
      std::vector<MatEntry>::const_iterator bRowIt;

      for (bRowIt = bIterator->begin();
	   bRowIt != bIterator->end();
	   ++bRowIt) {
	bool added = false;
	for (resultRowIt = resultIterator->begin();
	     resultRowIt != resultIterator->end();
	     ++resultRowIt) {
	  if (bRowIt->col == resultRowIt->col) {
	    resultRowIt->entry += bRowIt->entry;
	    
	    added = true;
	    break;
	  }
	}

	if (!added) {
	  resultIterator->push_back(*bRowIt);
	}
      }	         
    }      
  }

  void addDOFMatrix(DOFMatrix *result, const DOFMatrix *a)
  {
    DOFMatrix::Iterator resultIterator(result, USED_DOFS);
    DOFMatrix::Iterator aIterator(const_cast<DOFMatrix*>(a), USED_DOFS);

    for (resultIterator.reset(), aIterator.reset(); 
	 !aIterator.end(); 
	 ++resultIterator, ++aIterator) {
      std::vector<MatEntry>::iterator resultRowIt;
      std::vector<MatEntry>::const_iterator aRowIt;

      for (aRowIt = aIterator->begin();
	   aRowIt != aIterator->end();
	   ++aRowIt) {
	bool added = false;
	for (resultRowIt = resultIterator->begin();
	     resultRowIt != resultIterator->end();
	     ++resultRowIt) {
	  if (aRowIt->col == resultRowIt->col) {
	    resultRowIt->entry += aRowIt->entry;
	    
	    added = true;
	    break;
	  }
	}

	if (!added) {
	  resultIterator->push_back(*aRowIt);
	}
      }	         
    }      
  }
}