Sindbad~EG File Manager

/* Numerical matrix inversion via lu-decomposition */
/* Not put into deval.dll because I want deval.dll to be
   independent of heap.dll, and this file needs
   memory allocation, for which it uses heap.dll.
*/
/*
8.8.91  original date
6.14.98 last modified
*/

#define ALGEBRA_DLL
#include <string.h>
#include <assert.h>
#include <math.h>
#include "globals.h"
#include "ops.h"
#include "deval.h"
#include "matrix.h"   /* ludcmp */
#include "errbuf.h"

static void lubksb(double **, int, int *, double *);  /* Numerical Recipes */

#define ENTRY(i,j,t)  (ARG((j),ARG((i),(t))))
#define ENTRYPTR(i,j,t) (ARGPTR(ARG((i),(t))) + (j))
#define ROWS(t)  ARITY((t))
#define COLUMNS(t)  ARITY(ARG(0,(t)))  /* number of columns of a (non-bblocked) term */
/*_______________________________________________________________*/
MEXPORT_ALGEBRA int decimalmatrixinverse(term u, term arg, term *next, char *reason)
/* evaluate all entries of u to doubles if possible, compute the
inverse by a program from Numerical Recipes, then create a
MathXpert-style matrix of the result. */

{ unsigned short n;
  int i,j,err;
  double d;
  double **a,**y;
  double *col;
  int *indx;
  term t,temp;
  if(FUNCTOR(u) != MATRIXINVERSE)
     return 1;
  t = ARG(0,u);
  if(FUNCTOR(t) != MATRIX)
     return 1;
  if(ROWS(t) != COLUMNS(t))
     return 1;
  n = ROWS(t);
  col = (double *) callocate(n+1,sizeof(double));
  if(col==NULL)
     { nospace();
       return 1;
     }
  indx = (int *) callocate(n+1,sizeof(int));
  if(indx == NULL)
     { nospace();
       return 1;
     }
  a = (double **) callocate(n+1, sizeof(double *));
  if(a==NULL)
     { nospace();
       return 1;
     }
  for(i=1;i<=n;i++)
     { a[i] = (double *) callocate(n+1,sizeof(double));
       if(a[i]==NULL)
          { nospace();
            return 1;
          }
     }
  y = (double **) callocate(n, sizeof(double *));
  if(y==NULL)
     { nospace();
       return 1;
     }
  for(i=0;i<n;i++)
     { y[i] = (double *) callocate(n,sizeof(double));
       if(y[i]==NULL)
          { nospace();
            return 1;
          }
     }

/* Numerical Recipes wants subscripts that start at 1.  That's we we
   allocated n+1 instead of n spaces above. */

  for(i=1;i<=n;i++)
    { for(j=1;j<=n;j++)
        { err = deval(ENTRY(i-1,j-1,t), &(a[i][j]));
          if(err)
             { for(i=0;i<n;i++)
                   free2(y[i]);
               for(i=1;i<=n;i++)
                    free2(a[i]);
               free2(y); free2(a); free2(indx); free2(col);
               return 1;
             }
        }
    }
  /* Now invert the decimal matrix a */
  err = ludcmp(a,n,indx,&d);  /* from Numerical Recipes; see p. 45 of the book */
  if(err)
     { errbuf(0, english(1179));
         /* Matrix is singular (or very nearly so); */
       errbuf(1,english(1180));
         /* numerical inversion failed. */
       return 1;
     }
  for(j=0;j<n;j++)
     { for(i=1;i<=n;i++)
           col[i] = 0.0;
       col[j+1] = 1.0;
       lubksb(a,n,indx,col);
       for(i=0;i<n;i++) y[i][j] = col[i+1];
     }
  /* Now the matrix y (whose indices start at zero) holds the inverse.
     Convert it to a MathXpert-style matrix */
  *next = make_term(MATRIX,n);
  for(i=0;i<n;i++)
     { temp = make_term(VECTOR,n);
       for(j=0;j<n;j++)
          ARGREP(temp,j,make_double(y[i][j]));
       ARGREP(*next,i,temp);
     }
  HIGHLIGHT(*next);
  strcpy(reason, english(1181)); /* compute inverse */
  for(i=0;i<n;i++)
     free2(y[i]);
  for(i=1;i<=n;i++)
     free2(a[i]);
  free2(y); free2(a); free2(indx); free2(col);
  return 0;
}

/*_______________________________________________________________*/
static void lubksb(double **a,int n,int *indx,double *b)
/* Code adapted from Numerical Recipes */
{
  int i,ii=0,ip,j;
  double sum;
  for (i=1;i<=n;i++)
        {
           ip=indx[i];
           sum=b[ip];
           b[ip]=b[i];
           if (ii)
              {for (j=ii;j<i;j++)
                 sum -= a[i][j]*b[j];
              }
           else if (sum) ii=i;
           b[i]=sum;
        }
  for (i=n;i>=1;i--)
        {
           sum=b[i];
           for (j=i+1;j<=n;j++)
              sum -= a[i][j]*b[j];
           b[i]=sum/a[i][i];
        }
}