matrix.c

cfd求解器使用与gmsh网格的求解

#define RCSID "$Id: Matrix.c,v 1.25 2006/02/28 12:16:29 geuzaine Exp $"
/*
 * Copyright (C) 1997-2006 P. Dular, C. Geuzaine
 *
 * 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.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
 * USA.
 *
 * Please report all bugs and problems to <[e-mail]>.
 *
 * Contributor(s):
 *   Jean-Francois Remacle
 *   Benoit Meys
 *   Johan Gyselinck
 *   Ruth Sabariego
 */

#include <stdio.h>
#include <string.h>
#include <math.h>

#include "Solver.h"
#include "Solver_F.h"
#include "Message.h"
#include "Malloc.h"

/* ------------------------------------------------------------------------ */
/*  i n i t                                                                 */
/* ------------------------------------------------------------------------ */

void init_matrix (int NbLines, Matrix *M, Solver_Params *p){
  int i, j=0;

  M->T = p->Matrix_Format;  
  M->N = NbLines;
  M->changed = 1 ;
  M->ILU_Exists = 0;
  M->notranspose = 0 ;

  switch (M->T) {
  case SPARSE :
    M->S.a    = List_Create (NbLines, NbLines, sizeof(double));
    M->S.ai   = List_Create (NbLines, NbLines, sizeof(int));
    M->S.ptr  = List_Create (NbLines, NbLines, sizeof(int));
    M->S.jptr = List_Create (NbLines+1, NbLines, sizeof(int)); 
    /* '+1' indispensable: csr_format ecrit 'nnz+1' dans jptr[NbLine] */
    for(i=0; i<NbLines; i++) List_Add (M->S.jptr, &j);
    break;
  case DENSE :
    M->F.LU_Exist = 0;
    /* Tous les algos iteratifs sont programmes pour resoudre 
       A^T x = b... C'est tres con, mais bon. L'algo LU est le seul
       qui demande la vraie matrice en entree... */
    if(M->T == DENSE && p->Algorithm == LU){
      M->F.lu   = (double*) Malloc (NbLines * NbLines * sizeof(double));
      M->notranspose = 1 ;
    }
    M->F.a    = (double*) Malloc (NbLines * NbLines * sizeof(double));
    break;
  default :
    Msg(GERROR, "Unknown type of matrix storage format: %d", M->T);
    break;
  }
}


void init_vector (int Nb, double **V){
  *V = (double*) Malloc (Nb * sizeof(double)); 
}


/* ------------------------------------------------------------------------ */
/*  z e r o                                                                 */
/* ------------------------------------------------------------------------ */

void zero_matrix (Matrix *M){
  int i,j=0;

  M->changed = 1 ;

  switch (M->T) {
  case SPARSE :
    List_Reset (M->S.a);
    List_Reset (M->S.ai);
    List_Reset (M->S.ptr);
    List_Reset (M->S.jptr);
    for (i=0; i<M->N; i++) List_Add (M->S.jptr, &j);
    break;
  case DENSE :
    for(i=0; i<(M->N)*(M->N); i++) M->F.a[i] = 0.0;
    break;    
  }
}

void zero_matrix2 (Matrix *M){
  int      i, iptr;
  int     *jptr, *ptr;
  double  *a;

  M->changed = 1 ;
  switch (M->T) {
  case SPARSE :
    jptr  = (int*) M->S.jptr->array;
    ptr = (int*) M->S.ptr->array;
    a   = (double*) M->S.a->array;
    for (i=0; i<M->N; i++) {
      iptr = jptr[i];
      while (iptr>0) {
	a[iptr-1]= 0. ; 
	iptr = ptr[iptr-1];
      }
    }
    break;
  case DENSE :
    for(i=0; i<(M->N)*(M->N); i++) M->F.a[i] = 0.0;
    break;    
  }
}

void zero_vector (int Nb, double *V){
  int i;
  for(i=0; i<Nb; i++) V[i] = 0.0;
}


/* ------------------------------------------------------------------------ */
/*  c o p y                                                                 */
/* ------------------------------------------------------------------------ */

void copy_vector (int Nb, double *U, double *V){
  int i;
  for(i=0; i<Nb; i++) V[i] = U[i];
}

/* ------------------------------------------------------------------------ */
/*  a d d                                                                   */
/* ------------------------------------------------------------------------ */

void add_vector_vector (int Nb, double *U, double *V){
  /* u+v -> u */
  int i;
  for(i=0; i<Nb; i++) U[i] += V[i];
}

void add_vector_prod_vector_double (int Nb, double *U, double *V, double d){
  /* u+v*d -> u */
  int i;
  for(i=0; i<Nb; i++) U[i] += d*V[i];
}

void add_matrix_double (Matrix *M, int ic, int il, double val){
  /* attention a la transposition ! */
  int     *ai, *pp, n, iptr, iptr2, jptr, *ptr, zero = 0;
  double   *a;

  M->changed = 1 ;

  switch (M->T) {

  case SPARSE :
    il--;
    pp  = (int*) M->S.jptr->array;
    ptr = (int*) M->S.ptr->array;
    ai  = (int*) M->S.ai->array;
    a   = (double*) M->S.a->array;
    
    iptr = pp[il];
    iptr2 = iptr-1;
    
    while(iptr>0){
      iptr2 = iptr-1;
      jptr = ai[iptr2];
      if(jptr == ic){
	a[iptr2] += val;
	return;
      }
      iptr = ptr[iptr2];
    }

    List_Add (M->S.a, &val);
    List_Add (M->S.ai, &ic);
    List_Add (M->S.ptr, &zero);
    
    /* Les pointeurs ont pu etre modifies
       s'il y a eu une reallocation dans List_Add */
    
    ptr = (int*) M->S.ptr->array;
    ai  = (int*) M->S.ai->array;
    a   = (double*) M->S.a->array;
    
    n = List_Nbr(M->S.a);
    if(!pp[il]) pp[il] = n;
    else ptr[iptr2] = n;
    break;

  case DENSE :
    if(M->notranspose)
      M->F.a[((M->N))*(il-1)+(ic-1)] += val;
    else
      M->F.a[((M->N))*(ic-1)+(il-1)] += val;
    break;

  }
}


void add_matrix_matrix (Matrix *M, Matrix *N){
  /* M+N -> M */
  int      i, *ai, iptr, *jptr, *ptr;
  double  *a;

  switch (M->T) {
  case SPARSE :
    jptr = (int*) N->S.jptr->array;
    ptr  = (int*) N->S.ptr->array;
    a    = (double*) N->S.a->array;
    ai   = (int*) N->S.ai->array;

    for (i=0; i<N->N; i++) {
      iptr = jptr[i];
      while (iptr>0) {
	add_matrix_double (M, ai[iptr-1], i+1, a[iptr-1]); 
	/* add_matrix_double transpose, donc pour additionner,
	   il faut transposer une seconde fois */
	iptr = ptr[iptr-1];
      }
    }

    break;
  case DENSE :
    for(i=0; i<(M->N)*(M->N); i++) M->F.a[i] += N->F.a[i];
    break;
    
  }
}

void add_matrix_prod_matrix_double (Matrix *M, Matrix *N, double d){
  /* M+N*d -> M */
  int      i, *ai, iptr, *jptr, *ptr;
  double  *a;

  switch (M->T) {
  case SPARSE :
    jptr = (int*) N->S.jptr->array;
    ptr  = (int*) N->S.ptr->array;
    a    = (double*) N->S.a->array;
    ai   = (int*) N->S.ai->array;

    for (i=0; i<N->N; i++) {
      iptr = jptr[i];
      while (iptr>0) {
	add_matrix_double (M, ai[iptr-1], i+1, d*a[iptr-1]); 
	/* add_matrix_double transpose, donc pour additionner,
	   il faut transposer une seconde fois */
	iptr = ptr[iptr-1];
      }
    }

    break;
  case DENSE :
    for(i=0; i<(M->N)*(M->N); i++) M->F.a[i] += d*N->F.a[i];
    break;
    
  }
}

/* ------------------------------------------------------------------------ */
/*  s u b                                                                   */
/* ------------------------------------------------------------------------ */

void sub_vector_vector_1 (int Nb, double *U, double *V){
  /* u-v -> u */
  int i;
  for(i=0; i<Nb; i++) U[i] -= V[i];
}


void sub_vector_vector_2 (int Nb, double *U , double *V ){
  /* u-v -> v */
  int i;
  for(i=0; i<Nb; i++) V[i] = U[i] - V[i];
}


/* ------------------------------------------------------------------------ */
/*  p r o d                                                                 */
/* ------------------------------------------------------------------------ */

void prod_vector_double (int Nb, double *U, double a){
  /* u*a -> u  */
  int i;
  for(i=0; i<Nb; i++) U[i] *= a;
}

void prodsc_vector_vector (int Nb, double *U, double *V, double *prosca){
  /* u*v -> prosca  */
  int i;
  *prosca = 0.0 ;
  for (i=0; i<Nb; i++) *prosca += U[i] * V[i];
}



void scale_matrix (int scaling, Matrix *M){
  int     i, *ai, *jptr ;
  double  *a, *rowscal = NULL, *colscal = NULL;
  int job0=0, job1=1,  ioff=0, len, *idiag, norm ;


  switch (M->T) {

  case SPARSE :

    jptr = (int*) M->S.jptr->array;
    a    = (double*) M->S.a->array;
    ai   = (int*) M->S.ai->array;

    switch (scaling) {

    case DIAG_SCALING :

      rowscal = colscal = (double*)Malloc(M->N * sizeof(double));

      /* extract diagonal */
      idiag = (int*)Malloc(M->N * sizeof(int));
      getdia_ (&M->N, &M->N, &job0, a, ai, jptr, &len, rowscal, idiag, &ioff) ;
      Free (idiag);

      for (i = 0 ; i < M->N ; i++){
	if (rowscal[i]){
	  rowscal[i] = 1./sqrt(fabs(rowscal[i])) ;
	  /* printf("  %d %e \n", i, rowscal[i] ); */
	}
	else {
	  Msg(WARNING, "Diagonal scaling aborted because of zero diagonal element (%d)",i+1) ;
	  Free (rowscal) ;
	  return ;
	}
      }

      diamua_ (&M->N, &job1, a, ai, jptr, rowscal, a, ai, jptr) ;
      amudia_ (&M->N, &job1, a, ai, jptr, colscal, a, ai, jptr) ;
      break ;
      
    case MAX_SCALING   :  case NORM1_SCALING :  case NORM2_SCALING :

      switch (scaling) {
      case MAX_SCALING   : norm = 0 ; break ;
      case NORM1_SCALING : norm = 1 ; break ;
      case NORM2_SCALING : norm = 2 ; break ;
      }

      rowscal = (double*)Malloc(M->N * sizeof(double));
      rnrms_ (&M->N, &norm, a, ai, jptr, rowscal); 
      for (i = 0 ; i < M->N ; i++){
	/* printf("  %d %e \n", i, rowscal[i] ); */
	if (rowscal[i])
	  rowscal[i] = 1./rowscal[i] ;
	else {
	  Msg(WARNING, "Scaling aborted because of zero row (%d)", i+1) ;
	  Free (rowscal) ;
	  return ;
	}
      }
      diamua_ (&M->N, &job1, a, ai, jptr, rowscal, a, ai, jptr) ;
      
      colscal = (double*)Malloc(M->N * sizeof(double));
      cnrms_ (&M->N, &norm, a, ai, jptr, colscal); 
      for (i = 0 ; i < M->N ; i++){
	if (colscal[i]){
	  colscal[i] = 1./colscal[i] ;

	  /* printf("  %d %e %e \n", i, 1./rowscal[i], 1./colscal[i] ); */
	}
	else {
	  Msg(WARNING, "Scaling aborted because of zero column (%d)", i+1) ;
	  Free (colscal) ;
	  return ;
	}
      }
      amudia_ (&M->N, &job1, a, ai, jptr, colscal, a, ai, jptr) ;
      break;

    default : 

      Msg(GERROR, "Unknown type of matrix scaling: %d", scaling);
      break;

    }
   
    M->scaled = 1 ; 
    M->rowscal = rowscal ;
    M->colscal = colscal ;
   
    break;

  case DENSE :
    Msg(WARNING, "Scaling is not implemented for dense matrices") ;
    break;
  }

}


void scale_vector (int ROW_or_COLUMN, Matrix *M, double *V){
  double *scal = NULL;
  int i;

  if (!M->scaled) return ;
  
  switch (ROW_or_COLUMN) {
  case 0  : scal =  M->rowscal ; break ;
  case 1  : scal =  M->colscal ; break ;
  }

  if (scal == NULL) Msg(GERROR, "scale_vector : no scaling factors available !") ;

  for (i = 0 ; i < M->N ; i++) V[i] *= scal[i] ; 
}


void prod_matrix_vector (Matrix *M, double *V , double *res ){
  /* M*V -> res  ou M est la transposee!! */
  int     k, i, j, *ai, *jptr ;
  double  *a;

  switch (M->T) {
  case SPARSE :
    /* csr_format transpose! 
       donc la matrice arrivant dans cette routine doit
       bel et bien etre la transposee !!! */
    if(M->changed){
      csr_format (&M->S, M->N);
      restore_format (&M->S);
      M->changed = 0 ;
    }
    jptr = (int*) M->S.jptr->array;
    a    = (double*) M->S.a->array;
    ai   = (int*) M->S.ai->array;

    for(i=0; i<M->N; i++){
      res[i] = 0.0 ;
      for(k=jptr[i]; k<=jptr[i+1]-1; k++){
	res[i] += V[ai[k-1]-1] * a[k-1];
      }
    }

    break;
  case DENSE :
    if(M->notranspose){
      for(i=0; i<M->N; i++){
	res[i] = 0.0 ;
	for(j=0; j<M->N; j++) res[i] += M->F.a[(M->N)*i+j] * V[j];
      }
    }
    else{
      for(i=0; i<M->N; i++){
	res[i] = 0.0 ;
	for(j=0; j<M->N; j++) res[i] += M->F.a[(M->N)*j+i] * V[j];
      }
    }
    break;
  }
}


void prod_matrix_double (Matrix *M, double v){  
  /* M*v -> M */
  int i;
  double *a;

  switch (M->T) {
  case SPARSE :
    a = (double*) M->S.a->array;
    for(i=0; i<List_Nbr(M->S.a); i++){
      a[i] *= v;
    }
    break;
  case DENSE :
    for(i=0; i<(M->N)*(M->N); i++) M->F.a[i] *= v;    
    break;
  }

}

void multi_prod_matrix_double(int n, Matrix **Mat, double *coef, Matrix *MatRes){
  int k;

  zero_matrix(MatRes);
  for(k=0;k<n;k++){
    if(coef[k]){
      prod_matrix_double(Mat[k],coef[k]);
      add_matrix_matrix(MatRes,Mat[k]);
      prod_matrix_double(Mat[k],1.0/coef[k]);