Logo Search packages:      
Sourcecode: felt version File versions  Download package

factor.c

/*
    This file is part of the FElt finite element analysis package.
    Copyright (C) 1993-2000 Jason I. Gobat and Darren C. Atkinson

    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., 675 Mass Ave, Cambridge, MA 02139, USA.
*/

/************************************************************************
 * File:    factor.c
 *    
 * Description:   
 *          
 ************************************************************************/

# include <stdio.h>
# include <math.h>
# include <malloc.h>
# include "matrix.h"
# include "error.h"

# define SIGN(x) ((x) < 0 ? -1 : 1)

static void HouseHolderRow (a, v, j, m, n, w)
   Matrix   a;
   double   *v;
   unsigned m, n, j;
   double   *w;
{
   unsigned i, k;
   double   beta;
   double   dot;

   dot = 0;
   for (i = j ; i <= m ; i++)
      dot += v[i]*v[i];

   beta = -2.0/dot; 

   for (i = j ; i <= n ; i++) {
      w [i] = 0;
      for (k = j ; k <= m ; k++) 
         w [i] += v[k]*mdata(a, k, i);

      w [i] *= beta;
   }

   for (i = j ; i <= m ; i++) 
      for (k = j ; k <= n ; k++)
         sdata(a,i,k) += v[i]*w[k];
 
   return;
}

static void HouseHolder (v, a, j, m)
   double   *v;
   Matrix   a;
   unsigned j, m; 
{
   unsigned i;
   double   mu, beta;

   mu = 0;
   for (i = j ; i <= m ; i++) {
      v[i] = mdata(a, i, j);
      mu += v[i]*v[i];
   }

   mu = sqrt(mu);

   if (mu != 0) {
      beta = mdata(a, j, j) + SIGN(mdata(a, j, j))*mu;
      for (i = j+1 ; i <= m ; i++)
         v[i] /= beta;
   }

   v[j] = 1; 

   return;
}

int QRFactorMatrix (q, r, a)  /* factor a such that qTa = r       */
   Matrix   q;          /* destination matrix for q         */
   Matrix   r;          /* destination matrix for r         */
   Matrix   a;          /* source matrix              */
{
   unsigned i, j;
   unsigned m , n;
   double   *v, *w;
   int            status;

   if (IsCompact(q) || IsCompact(r))
      return M_COMPACT;

   if (Mrows(a) < Mcols(a))
      return M_NOTSQUARE;

   if (Mrows(a) != Mrows(r) || Mrows(r) != Mrows(q))
      return M_SIZEMISMATCH;

   if (Mcols(a) != Mcols(r) || Mcols(r) != Mcols(q))
      return M_SIZEMISMATCH;

   if (a == q)          /* can overwrite a with r but not with q */
      return M_NOOVERWRITE;

   if (a != r) {
      status = CopyMatrix (r, a);
      if (status)
         return status;
   }

   status = IdentityMatrix (q);
   if (status)
      return status;

   m = Mrows(r);
   n = Mcols(r);

   v = (double *) malloc (sizeof(double) * m);
   if (v == NULL)
      Fatal ("unable to allocate temporary vector");
   v --;
   w = (double *) malloc (sizeof(double) * m);
   if (w == NULL)
      Fatal ("unable to allocate temporary vector");
   w --;

   for (j = 1 ; j <= n ; j++) {
      HouseHolder (v, r, j, m);
      HouseHolderRow (r, v, j, m, n, w);       
      
      if (j < m) {
         for (i = j+1 ; i <= m ; i++)
            sdata(r,i,j) = v[i];
      }
   }

   for (j = n ; j >= 1 ; j--) {
      v [j] = 1;
      for (i = j+1 ; i <= m ; i++)
         v [i] = mdata(r, i, j);

      HouseHolderRow (q, v, j, m, m, w);
   }

   v++; free(v);
   w++; free(w);

   return 0;
}

int CholeskyFactorMatrix (b, a)     /* factor a such that bbT=a   */
   Matrix   b;                /* destination matrix         */
   Matrix   a;                /* source matrix        */
{
   unsigned i, j, k;
   unsigned n;
   double   t;
   int            status;

   if (IsCompact(b))
      return M_COMPACT;

   if (Mrows(a) != Mcols(a))
      return M_NOTSQUARE;

   if (Mrows(a) != Mrows(b) || Mcols(a) != Mcols(b))
      return M_SIZEMISMATCH;

   if (!IsSymmetricMatrix (a))
      return M_NOTSYMMETRIC;

   if (a != b) {
      status = CopyMatrix (b, a);
      if (status)
         return status;
   }

   n = Mrows(b);

   for (k = 1 ; k <= n ; k++) {
      t = mdata(b, k, k);
      if (t <= 0)
         return M_NOTPOSITIVEDEFINITE;
 
      t = sqrt (t);
      sdata(b,k,k) = t;

      for (j = k+1 ; j <= n ; j++)
         sdata(b,k,j) /= t;
/*
         sdata(b,j,k) /= t;
*/

      for (j = k+1 ; j <= n ; j++) {
         t = sdata(b,k,j);
/*
         t = sdata(b,j,k); 
*/
         for (i = j ; i <= n ; i++) 
            sdata(b,i,j) -= t*mdata(b, k, i);
/*
            sdata(b,i,j) -= t*mdata(b, i, k);
*/
      }
   }

   for (i = 1 ; i <= n ; i++)
      for (j = i+1 ; j <= n ; j++)
         sdata(b,j,i) = 0.0;
/*
         sdata(b,i,j) = 0.0;
*/

   return 0;
}

int InvertMatrix (b, a, p)    /* compute b = inv(a) where a is LU factored */
   Matrix   b;          /* destination matrix                    */
   Matrix   a;          /* factored source matrix                */
   Matrix   p;          /* pivot vector                          */
{
   unsigned i, j;
   unsigned n;
   Matrix   work;

   if (IsCompact(b))
      return M_COMPACT;

   if (Mrows(a) != Mcols(a))
      return M_NOTSQUARE;

   if (Mrows(a) != Mrows(p) || Mcols(p) != 1)
      return M_SIZEMISMATCH; 

   if (Mrows(a) != Mrows(b) || Mcols(a) != Mcols(b))
      return M_SIZEMISMATCH;

   if (a == b) 
      return M_NOOVERWRITE;

   n = Mrows(a);
   work = CreateColumnVector (n);

   for (i = 1 ; i <= n ; i++) {
      ZeroMatrix (work);
      sdata(work, i, 1) = 1.0;
      LUBackSolveMatrix(work, a, work, p);
      for (j = 1 ; j <= n ; j++)
         sdata(b,j,i) = mdata(work,j,1);
   }
   
   DestroyMatrix (work);
   return 0;
}

int DeterminantMatrix (result, a, p)      /* result = |a|               */
   double   *result;          /* pointer to result location */
   Matrix   a;                /* factorized source matrix   */
   Matrix   p;                /* pivot vector               */
{
   unsigned i;
   double   x, y;
   unsigned n;

   if (Mrows(a) != Mcols(a))
      return M_NOTSQUARE;

   if (Mrows(a) != Mrows(p) || Mcols(p) != 1)
      return M_SIZEMISMATCH;

   n = Mrows(a);

   x = 1.0;
   y = 0.0;
   for (i = 1 ; i <= n ; i++) {
      if ((int) mdata(p, i, 1) != i) 
         x = -x;

      x *= mdata(a, i, i);
      if (x != 0) {
         while (fabs (x) < 1.0) {
            x *= 10.0;
            y -= 1.0;
         }

         while (fabs (x) >= 10.0) {
            x /= 10.0;
            y += 1.0;
         }
      }
      else
         break;
   }

   *result = x*pow(10.0, y);
         
   return 0;
}

int LUFactorMatrix (b, a, p, info) /* factor a into LU and store in b   */
   Matrix   b;              /* destination matrix           */
   Matrix   a;              /* source matrix                */
   Matrix   p;              /* permutation vector           */
   int            *info;                /* singularity code             */
{
   double   t;
   unsigned i, j, k;
   unsigned n;
   double   max;
   int            status;
   unsigned mu;

   if (IsCompact(b))
      return M_COMPACT;

   if (Mrows(a) != Mcols(a))
      return M_NOTSQUARE;

   if (Mrows(a) != Mrows(p))
       return M_SIZEMISMATCH;

   if (Mrows(a) != Mrows(b) || Mcols(a) != Mcols(b))
      return M_SIZEMISMATCH;

   if (a != b) {
      status = CopyMatrix (b, a);
      if (status)
         return status;
   }

   *info = 0;

   n = Mrows(b);

   for (j = 1 ; j <= n ; j++)
      sdata(p, j, 1) = j;

   for (j = 1 ; j <= n ; j++) {

      for (k = 1 ; k <= j-1 ; k++) { 
         mu = mdata(p, k, 1);
         if (mu != k) {
            t = mdata(b, k, j);
            sdata(b,k,j) = mdata(b,mu,j);
            sdata(b,mu,j) = t;
         }
      }

      for (k = 1 ; k <= j-1 ; k++) 
         for (i = k+1 ; i <= j-1 ; i++) 
            sdata(b,i,j) = mdata(b,i,j) - mdata(b,i,k)*mdata(b,k,j);

      for (k = 1 ; k <= j-1 ; k++)
         for (i = j ; i <= n ; i++)
            sdata(b,i,j) = mdata(b,i,j) - mdata(b,i,k)*mdata(b,k,j);

      mu = j;
      max = fabs (mdata(b,j,j));
      for (k = j ; k <= n ; k++) {
         if ((t = fabs (mdata(b,k,j))) > max) {
            max = t;
            mu = k;
         }
      } 

      sdata(p,j,1) = mu;

      if (j != mu) {
         for (k = 1 ; k <= j ; k++) {
            t = mdata(b, j, k);
            sdata(b,j,k) = mdata(b,mu,k);
            sdata(b,mu,k) = t;
         }
      }

      if (mdata(b,j,j) != 0) {
         t = mdata(b, j, j);
         for (i = j+1 ; i <= n ; i++)
            sdata(b,i,j) = mdata(b, i, j) / t;
      }
      else
         *info = j;
   }
                  
   return 0; 
}

int FormLUPMatrices (L, U, P, a, p) /* separate matrices from factors    */
   Matrix   L;                /* return matrix for L             */
   Matrix   U;                /* return matrix for U             */
   Matrix   P;                /* return matrix for P             */
   Matrix   a;                /* factored form of matrix         */
   Matrix   p;                /* pivot vector                    */
{
   unsigned i, j;
   unsigned n;
   unsigned k_pvt;
   unsigned temp;
   unsigned *pivot;

   if (IsCompact(L) || IsCompact(U) || IsCompact(P))
      return M_COMPACT;

   if (!IsSquare(a))
      return M_NOTSQUARE;

   if (Mrows(a) != Mrows(p) || Mcols(p) != 1)
      return M_SIZEMISMATCH;

   if (Mrows(a) != Mrows(L) || Mrows(a) != Mrows(U) || Mrows(a) != Mrows(P))
      return M_SIZEMISMATCH;

   if (Mcols(a) != Mcols(L) || Mcols(a) != Mcols(U) || Mcols(a) != Mcols(P))
      return M_SIZEMISMATCH;

   n = Mrows(a);

   pivot = (unsigned *) malloc (sizeof (unsigned) * n); pivot --;

   for (i = 1 ; i <= n ; i++) 
      pivot [i] = i;

   for (i = 1 ; i < n ; i++) {
      k_pvt = (unsigned) mdata(p,i,1);

      if (k_pvt != i) {
         temp = pivot [k_pvt];
         pivot [k_pvt] = pivot [i];
         pivot [i] = temp;
      }
   }

   ZeroMatrix (P);
   ZeroMatrix (L);
   ZeroMatrix (U);

   for (i = 1 ; i <= n ; i++) {
      sdata(P,i,pivot[i]) = 1.0;

      sdata(L,i,i) = 1.0;
      for (j = 1 ; j < i ; j++)
         sdata(L,i,j) = mdata(a,i,j);

      for (j = i ; j <= n ; j++)
         sdata(U,i,j) = mdata(a,i,j);
   }

   pivot ++; free (pivot);

   return 0;
}

int LUBackSolveMatrix (c, a, b, p)  /* solve Ax=b and store result in c  */   
   Matrix   c;                /* destination vector              */
   Matrix   a;                /* factorized source matrix          */   
   Matrix   b;                /* right hand side vector          */
   Matrix   p;                /* pivot vector                    */
{
   unsigned i, k;
   unsigned n;
   int            mu;
   double   t;
   int            status;

   if (Mrows(a) != Mcols(a))
      return M_NOTSQUARE;

   if (Mcols(b) != 1 || Mcols(p) != 1 || Mcols(c) != 1)
      return M_SIZEMISMATCH;

   if (Mrows(a) != Mrows(p) || Mrows(a) != Mrows(b) || Mrows(b) != Mrows(c))
      return M_SIZEMISMATCH; 

   if (b != c) {
      status = CopyMatrix (c, b);
      if (status)
         return status;
   }

   n = Mrows(a);

      /*
       * pivot the RHS vector to form x = Pb
       */

   for (i = 1 ; i <= n ; i++) {
      mu = (int) mdata(p, i, 1);
      if (mu != i) {
         t = mdata(c, i, 1);
         sdata(c,i,1) = mdata(c, mu, 1);
         sdata(c,mu,1) = t;
      }
   }

      /*
       * Solve Ly = x and store y in c
       */

   for (k = 1 ; k <= n ; k++) {
      if (mdata(c, k, 1) != 0) {
         for (i = k+1 ; i <= n ; i++) 
            sdata(c,i,1) = mdata(c, i, 1) - mdata(c, k, 1)*mdata(a, i, k);
      }
   }

      /*
       * Solve Uc = y
       */

   for (k = n ; k >= 1 ; k--) {
      if (mdata(c, k, 1) != 0) 
         sdata(c,k,1) = mdata(c, k, 1)/mdata(a, k, k);

      for (i = 1 ; i <= k-1 ; i++)
        sdata(c,i,1) = mdata(c, i, 1) - mdata(c, k, 1)*mdata(a, i, k);
   }
   
   return 0;
}

int CroutFactorMatrix (A)     /* crout factorize A and store in A */
   Matrix   A;          /* source and destination matrix    */
{
   unsigned     j,jj,jjlast,jcolht,
            istart,ij,ii,i,
            icolht,iilast,
            length,jtemp,jlngth;
   double   temp, dot;
   unsigned n, k;

   if (IsFull(A))
      return M_NOTCOMPACT;
 
   if (Mrows(A) != Mcols(A))
      return M_NOTSQUARE;
  
   if (IsSubsection (A))
      return M_SUBSECTION;

   n = Mrows(A);

   jj = 0;
   for (j = 1; j <= n; j++) {

      jjlast = jj;
      jj = A -> diag [j];
      jcolht = jj - jjlast;

      if (jcolht > 2) {
         
         istart = j - jcolht + 2;
         ij = jjlast + 2;
         ii = A -> diag [istart-1];

         for (i = istart; i <= j - 1 ; i++) {

            iilast = ii;
            ii = A -> diag [i];
            icolht = ii - iilast;
            jlngth = i - istart + 1;
            if (icolht - 1  < jlngth) 
               length = icolht - 1;
            else
               length = jlngth;
            
            if (length > 0) {
               dot = 0;
               for (k = 0 ; k < length ; k++)
                  dot += A -> data [ii-length+k][1] * A -> data[ij-length+k][1];

               A -> data [ij][1] -= dot;
            }

            ij++;
         }
      }

      if (jcolht >= 2) {

         jtemp = j - jj;
         for (ij = jjlast+1 ; ij <= jj-1 ; ij++) {

            ii = A -> diag [jtemp + ij];
           
            if (A -> data [ii][1] != 0.0) {
               temp = A -> data [ij][1];
               A -> data [ij][1] = temp / A -> data [ii][1];
               A -> data [jj][1] -= temp*A -> data [ij][1];
            }
         }
      }
   }

   return 0;
}

int CroutBackSolveMatrix (A, b)     /* solve Ax=b and store x in b      */
   Matrix          A;               /* Crout factored LHS matrix  */
   Matrix          b;               /* RHS (and dest) vector      */
{
   unsigned  jj,j,jjlast,
             jcolht,jjnext,
             istart,jtemp,i;
   double    Ajj;
   unsigned  n, k;
   double    dot;

   if (IsFull(A))
      return M_NOTCOMPACT;

   if (Mrows(A) != Mcols(A))
      return M_NOTSQUARE;
    
   if (Mrows(A) != Mrows(b))
      return M_SIZEMISMATCH;

   if (IsSubsection (A) || IsSubsection (b))
      return M_SUBSECTION;

   n = Mrows(A);

   jj = 0;
   for (j = 1 ; j <= n ; j++) {

      jjlast = jj;
      jj = A -> diag [j];
      jcolht = jj - jjlast;

      if (jcolht > 1) {
         dot = 0;
         for (k = 0 ; k < jcolht-1 ; k++)
            dot += A -> data [jjlast+1+k][1] * b -> data [j-jcolht+1+k][1];

         b -> data [j][1] -= dot;
      }
   }

   for (j = 1 ; j <= n ; j++) {
      Ajj = A -> data [A -> diag[j]][1];
      if (Ajj != 0.0)
         b -> data [j][1] /= Ajj;
   }

   if (n == 1)
      return 0;

   jjnext = A -> diag [n];

   for (j = n ; j >= 2 ; j--) {

      jj = jjnext;
      jjnext = A -> diag [j-1];
      jcolht = jj - jjnext;
      if (jcolht > 1) {

          istart = j - jcolht + 1;
          jtemp = jjnext - istart + 1;

          for (i = istart ; i <= j-1 ; i++) 
             b -> data [i][1] -= A -> data [jtemp + i][1] * b -> data[j][1];
      }
   }

   return 0;
}

Generated by  Doxygen 1.6.0   Back to index