/*
 *  Matrix.c
 *
 *  Contains the standard library and user functions
 *  for Matrix.library
 *
 *  This code is FREELY DISTRIBUTABLE but NOT PUBLIC DOMAIN.
 *  (Written by Randy Finch)
 */

#include <exec/types.h>
#include <exec/libraries.h>
#include <exec/memory.h>
#include <libraries/dos.h>
#include <proto/exec.h>
#include <stdlib.h>
#include <mieeedoub.h>

/* Useful definitions for allocating vectors and matrices */

#define ChkLVector(v,els) \
	 {if(!v) { v = AllocLVector(els); if(!v) return NULL; } }
#define ChkDVector(v,els) \
	 {if(!v) { v = AllocDVector(els); if(!v) return NULL; } }
#define ChkLMatrix(m,rows,cols) \
	 {if(!m) { m = AllocLMatrix(rows,cols); if(!m) return NULL; } }
#define ChkDMatrix(m,rows,cols) \
	 {if(!m) { m = AllocDMatrix(rows,cols); if(!m) return NULL; } }


/*
   The Extended Library structure is equivalent to the one in MatrixLib.a.
   Since a pointer is passed from assembly to the C routines, the same data
   can be manipulated from C or assembly.

   Make sure the two structures are EQUIVALENT!!!
*/

typedef struct Library	  LIB;

struct ExtLibrary {
   LIB lib;

   /* User data can go here */

   LONG seglist;
   LONG SysBase;
   LIB *MathIeeeDoubBasBase;
};

typedef struct ExtLibrary ELIB;

extern LIB *MathIeeeDoubBasBase;

#define VERSION     1
#define REVISION    0

/*
 *    The Initialization routine is given only the library pointer.
 *    Since we are NOT AUTOINIT we must add the library ourself
 *    and return the library pointer.  Exec has Forbid()
 *    for us during the call.
 *
 */

ELIB *CInit(lib)
   ELIB *lib;
{
   /* Here is where other libraries are opened and internal data
    * structures are initialized
    */

   MathIeeeDoubBasBase = OpenLibrary("mathieeedoubbas.library",0L);
   if(!MathIeeeDoubBasBase)  return (ELIB *)NULL;
   lib->MathIeeeDoubBasBase = MathIeeeDoubBasBase;

   AddLibrary((LIB *)lib);  /* Add ourself because we are not auto init*/
   return(lib);
}

/*
 *    Open is given the library pointer and the version request.  Either
 *    return the library pointer or NULL.  Remove the DELAYED-EXPUNGE flag.
 *    OpenLibrary() will check the version for us; we do not need to check.
 *    Exec has Forbid() for us during the call.
 */

ELIB *LibOpen(lib,version)
   ELIB *lib;
   long version;
{
   ++(lib->lib.lib_OpenCnt);
   lib->lib.lib_Flags &= ~LIBF_DELEXP;

   /* Here is where you can do whatever is needed each time the library
    * is opened.
    */

   return(lib);
}

/*
 *    Close is given the library pointer.  Be sure
 *    not to decrement the open count if already zero.	If the open count
 *    is or becomes zero AND there is a LIBF_DELEXP, we expunge the library
 *    and return the seglist.  Otherwise we return NULL.
 *
 *    Note that this routine never sets LIBF_DELEXP on its own.
 *
 *    Exec has Forbid() for us during the call.
 */

LibClose(lib)
   ELIB *lib;
{
   if (lib->lib.lib_OpenCnt) {
      --(lib->lib.lib_OpenCnt);
      return(NULL);
   }

   if (lib->lib.lib_Flags & LIBF_DELEXP)
      return(LibExpunge(lib));

   return(NULL);
}

/*
 *    We expunge the library and return the Seglist ONLY if the open count
 *    is zero.	If the open count is not zero we set the DELAYED-EXPUNGE
 *    flag and return NULL.
 *
 *    Exec has Forbid() for us during the call.  NOTE ALSO that Expunge
 *    might be called from the memory allocator and thus we CANNOT DO A
 *    Wait() or otherwise take a long time to complete (straight from RKM).
 *
 *    Apparently RemLibrary(lib) calls our expunge routine and would
 *    therefore freeze if we called it ourselves.  It appears that
 *    LibExpunge(lib) must remove the library itself as shown below.
 */

LibExpunge(lib)
   ELIB *lib;
{
   long seg;

   if (lib->lib.lib_OpenCnt) {
      lib->lib.lib_Flags |= LIBF_DELEXP;
      return(NULL);
   }

   /* Clean up yourself here */

   CloseLibrary(MathIeeeDoubBasBase);
   seg = lib->seglist;
   Remove((struct Node *)lib);
   return(seg);
}


/*----------------------------------------------------------------------*/

	  /* And now it is time to put in the USER FUNCTIONS */

/*----------------------------------------------------------------------*/

/*---------------------------------------------------
      Routines for LONG INTEGER vectors and matrices
-----------------------------------------------------*/

/* Allocate a long integer vector with numels number of elements
*/
LONG *AllocLVector(numels)
   LONG numels;
{
   LONG *v;

   if (numels <= 0L) return NULL;  /* Number of elements must be > zero */
   v = (LONG *)AllocMem(numels*sizeof(LONG),MEMF_CLEAR);

   return v;  /* v will be NULL if allocation failed */

} /* AllocLVector */


/* Allocate a long integer matrix
*/
LONG **AllocLMatrix (numrows, numcols)
   LONG numrows, numcols;
{
   LONG i, j;
   LONG **m;

   /* Number of rows and columns must be > zero */
   if ( (numrows <= 0L) || (numcols <= 0L) )  return NULL;

   /* Allocate row pointers */
   m = (LONG **)AllocMem(numrows*sizeof(LONG *),MEMF_CLEAR);
   if (!m)  return NULL;   /* allocation failed */

   /* Allocate row vectors and equate row pointers to them */
   for (i=0L ; i<numrows ; i++) {
      m[i] = (LONG *)AllocMem(numcols*sizeof(LONG),MEMF_CLEAR);
      if (!m[i]) {
	 for (j=0L ; j<i ; j++) {
	    FreeMem((void *)m[j],numcols*sizeof(LONG));
	 }
	 FreeMem((void *)m, numrows*sizeof(LONG *));
	 return NULL;  /* allocation failed */
      } /* if */
   } /* for */

   return m;

} /* AllocLMatrix */


/* Free a long integer vector
*/
BOOL FreeLVector(v, numels)
   LONG *v, numels;
{
   /* Make sure pointer is not NULL and numels is non-zero */
   if (v && numels) {
      FreeMem((void *)v, numels*sizeof(LONG));
      return TRUE;
   }

   return FALSE;

} /* FreeLVector */


/* Free a long integer matrix
*/
BOOL FreeLMatrix (m, numrows, numcols)
   LONG **m;
   LONG numrows, numcols;
{
   LONG i;

   /* Make sure pointer is not NULL and numrows and numcols are non-zero */
   if (m && numrows && numcols) {
      for (i=numrows-1 ; i>=0L ; i--) {
	 FreeMem((void *)m[i], numcols*sizeof(LONG));  /* Free elements */
      }
      FreeMem((void *)m, numrows*sizeof(LONG *));   /* Free row pointers */
      return TRUE;
   }

   return FALSE;

} /* FreeLMatrix () */


/* Add two long integer vectors
*/
LONG *AddLVectors (v1, v2, vr, numels)
   LONG *v1, *v2, *vr;
   LONG numels;
{
   LONG i;

   ChkLVector(vr,numels);
   for (i=0L ; i<numels ; i++) {
      vr[i] = v1[i] + v2[i];
   } /* for */
   return vr;

} /* AddLVectors () */


/* Subtract two long integer vectors
*/
LONG *SubLVectors (v1, v2, vr, numels)
   LONG *v1, *v2, *vr;
   LONG numels;
{
   LONG i;

   ChkLVector(vr,numels);
   for (i=0L ; i<numels ; i++) {
      vr[i] = v1[i] - v2[i];
   } /* for */
   return vr;

} /* SubLVectors () */


/* Add two long integer matrices
*/
LONG **AddLMatrices (m1, m2 , mr, numrows, numcols)
   LONG **m1, **m2, **mr;
   LONG numrows, numcols;
{
   LONG i, j;

   ChkLMatrix(mr,numrows,numcols);
   for (i=0L ; i<numrows ; i++) {
      for (j=0L ; j<numcols ; j++) {
	 (mr[i])[j] = (m1[i])[j] + (m2[i])[j];
      } /* for j */
   } /* for i */
   return mr;

} /* AddLMatrices () */


/* Subtract two long integer matrices
*/
LONG **SubLMatrices (m1, m2 , mr, numrows, numcols)
   LONG **m1, **m2, **mr;
   LONG numrows, numcols;
{
   LONG i, j;

   ChkLMatrix(mr,numrows,numcols);
   for (i=0L ; i<numrows ; i++) {
      for (j=0L ; j<numcols ; j++) {
	 (mr[i])[j] = (m1[i])[j] - (m2[i])[j];
      } /* for j */
   } /* for i */
   return mr;

} /* SubLMatrices () */


LONG **MultLMatrices(m1, m2, mr, numrows1, numcols1, numcols2)
   LONG **m1, **m2, **mr;
   LONG numrows1, numcols1, numcols2;
{
   LONG row1, col1, row2, col2;
   LONG numrows2;

   numrows2 = numcols1;  /* This MUST be true for multiplication */

   ChkLMatrix(mr,numrows1,numcols2);
   for (row1=0L ; row1<numrows1 ; row1++) {
      for (col2=0L ; col2<numcols2 ; col2++) {
	 /* Initialize in case mr has non-zero elements */
	 (mr[row1])[col2] = 0;
	 for (col1=0L,row2=0L ; (col1<numcols1)&&(row2<numrows2) ;
	       col1++,row2++) {
	    (mr[row1])[col2] += (m1[row1])[col1] * (m2[row2])[col2];
	 } /* for */
      } /* for */
   } /* for */
   return mr;

} /* MultLMatrices */


LONG *MultLVectorMatrix(v, m, vr, numels, numcols)
   LONG *v, **m, *vr;
   LONG numels, numcols; /* Elements in vector, columns in matrix */
{
   LONG mrow, mcol, vcol;
   LONG numrows;

   /* This function calculates v*m, thus v MUST be a ROW VECTOR */

   numrows = numels;  /* This MUST be true for multiplication */

   ChkLVector(vr,numcols);
   for (mcol=0L ; mcol<numcols ; mcol++) {
      /* Initialize in case vr has non-zero elements */
      vr[mcol] = 0;
      for (vcol=0L,mrow=0L ; vcol<numels ; vcol++,mrow++) {
	 vr[mcol] += v[vcol] * (m[mrow])[mcol];
      } /* for */
   } /* for */
   return vr;

} /* MultLVectorMatrix */


LONG *MultLMatrixVector(m, v, vr, numrows, numels)
   LONG *v, **m, *vr;
   LONG numels, numrows; /* Elements in vector, rows in matrix */
{
   LONG mrow, mcol, vrow;
   LONG numcols;

   /* This function calculates m*v, thus v MUST be a COLUMN VECTOR */

   numcols = numels;  /* This MUST be true for multiplication */

   ChkLVector(vr,numrows);
   for (mrow=0L ; mrow<numrows ; mrow++) {
      /* Initialize in case vr has non-zero elements */
      vr[mrow] = 0;
      for (vrow=0L,mcol=0L ; vrow<numels ; vrow++,mcol++) {
	 vr[mrow] += (m[mrow])[mcol] * v[vrow];
      } /* for */
   } /* for */
   return vr;

} /* MultLMatrixVector */


LONG **TransposeLMatrix(LONG **m, LONG **mt, LONG numrows, LONG numcols)
   /*	 m is the matrix to be transposed
	 mt is the transposed matrix
	 numrows is number of rows in m
	 numcols is number of columns in m
   */
{
   LONG i, j;

   /* If mt is NULL, client wants us to allocate.
      Notice that the number of rows and columns are reversed.
   */
   ChkLMatrix(mt,numcols,numrows);

   /* Transpose the matrix */
   for (i=0 ; i<numrows ; i++) {
      for (j=0 ; j<numcols ; j++) {
	 (mt[j])[i] = (m[i])[j];
      }
   }
   return mt;

}  /* TransposeLMatrix */



/*-------------------------------------------------
      Routines for DOUBLE vectors and matrices
---------------------------------------------------*/

/* Allocate a double vector with numels number of elements
*/
DOUBLE *AllocDVector(numels)
   LONG numels;
{
   DOUBLE *v;

   if (numels <= 0L) return NULL;  /* Number of elements must be > zero */

   v = (DOUBLE *)AllocMem(numels*sizeof(DOUBLE),MEMF_CLEAR);

   return v;  /* v will be NULL if allocation failed */

} /* AllocDVector */


/* Allocate a double matrix
*/
DOUBLE **AllocDMatrix (numrows, numcols)
   LONG numrows, numcols;
{
   LONG i, j;
   DOUBLE **m;

   /* Number of rows and columns must be > zero */
   if ( (numrows == 0L) || (numcols == 0L) )  return NULL;

   /* Allocate row pointers */
   m = (DOUBLE **)AllocMem(numrows*sizeof(DOUBLE *),MEMF_CLEAR);
   if (!m)  return NULL;   /* AllocMem failed */

   /* Allocate row vectors and equate row pointers to them */
   for (i=0L ; i<numrows ; i++) {
      m[i] = (DOUBLE *)AllocMem(numcols*sizeof(DOUBLE),MEMF_CLEAR);
      if (!m[i]) {
	 for (j=0L ; j<i ; j++) {
	    FreeMem((void *)m[j], numcols*sizeof(DOUBLE));
	 }
	 FreeMem((void *)m,numrows*sizeof(DOUBLE *));
	 return NULL;  /* AllocMem failed */
      } /* if */
   } /* for */

   return m;

} /* AllocDMatrix */


/* Free a double vector
*/
BOOL FreeDVector(v,numels)
   DOUBLE *v;
   LONG   numels;
{

   /* Make sure pointer is not NULL and numels is non-zero */
   if (v && numels) {
      FreeMem((void *)v,numels*sizeof(DOUBLE));
      return TRUE;
   }

   return FALSE;

} /* FreeDVector */


/* Free a double matrix
*/
BOOL FreeDMatrix (m, numrows, numcols)
   DOUBLE **m;
   LONG numrows, numcols;
{
   LONG i;

   /* Make sure pointer is not NULL and numrows and numcols are non-zero */
   if (m && numrows && numcols) {
      for (i=numrows-1 ; i>=0L ; i--) {
	 FreeMem((void *)m[i], numcols*sizeof(DOUBLE));  /* Free elements */
      }
      FreeMem ((void *)m, numrows*sizeof(DOUBLE *));   /* Free row pointers */
      return TRUE;
   }

   return FALSE;

} /* FreeDMatrix () */


/* Add two double vectors
*/
DOUBLE *AddDVectors (v1, v2, vr, numels)
   DOUBLE *v1, *v2, *vr;
   LONG numels;
{
   LONG i;

   ChkDVector(vr,numels);
   for (i=0L ; i<numels ; i++) {
      vr[i] = v1[i] + v2[i];
   } /* for */
   return vr;

} /* AddDVectors () */


/* Subtract two double vectors
*/
DOUBLE *SubDVectors (v1, v2, vr, numels)
   DOUBLE *v1, *v2, *vr;
   LONG numels;
{
   LONG i;

   ChkDVector(vr,numels);
   for (i=0L ; i<numels ; i++) {
      vr[i] = v1[i] - v2[i];
   } /* for */
   return vr;

} /* SubDVectors () */


/* Add two double matrices
*/
DOUBLE **AddDMatrices (m1, m2 , mr, numrows, numcols)
   DOUBLE **m1, **m2, **mr;
   LONG numrows, numcols;
{
   LONG i, j;

   ChkDMatrix(mr,numrows,numcols);
   for (i=0L ; i<numrows ; i++) {
      for (j=0L ; j<numcols ; j++) {
	 (mr[i])[j] = (m1[i])[j] + (m2[i])[j];
      } /* for j */
   } /* for i */
   return mr;

} /* AddDMatrices () */


/* Subtract two double matrices
*/
DOUBLE **SubDMatrices (m1, m2 , mr, numrows, numcols)
   DOUBLE **m1, **m2, **mr;
   LONG numrows, numcols;
{
   LONG i, j;

   ChkDMatrix(mr,numrows,numcols);
   for (i=0L ; i<numrows ; i++) {
      for (j=0L ; j<numcols ; j++) {
	 (mr[i])[j] = (m1[i])[j] - (m2[i])[j];
      } /* for j */
   } /* for i */
   return mr;

} /* SubDMatrices () */


DOUBLE **MultDMatrices(m1, m2, mr, numrows1, numcols1, numcols2)
   DOUBLE **m1, **m2, **mr;
   LONG numrows1, numcols1, numcols2;
{
   LONG row1, col1, row2, col2;
   LONG numrows2;

   numrows2 = numcols1;  /* This MUST be true for multiplication */

   ChkDMatrix(mr,numrows1,numcols2);
   for (row1=0L ; row1<numrows1 ; row1++) {
      for (col2=0L ; col2<numcols2 ; col2++) {
	 /* Initialize in case mr has non-zero elements */
	 (mr[row1])[col2] = 0;
	 for (col1=0L,row2=0L ; (col1<numcols1)&&(row2<numrows2) ;
	       col1++,row2++) {
	    (mr[row1])[col2] += (m1[row1])[col1] * (m2[row2])[col2];
	 } /* for */
      } /* for */
   } /* for */
   return mr;

} /* MultDMatrices */


DOUBLE *MultDVectorMatrix(v, m, vr, numels, numcols)
   DOUBLE *v, **m, *vr;
   LONG numels, numcols; /* Elements in vector, columns in matrix */
{
   LONG mrow, mcol, vcol;
   LONG numrows;

   /* This function calculates v*m, thus v MUST be a ROW VECTOR */

   numrows = numels;  /* This MUST be true for multiplication */

   ChkDVector(vr,numcols);
   for (mcol=0L ; mcol<numcols ; mcol++) {
      /* Initialize in case vr has non-zero elements */
      vr[mcol] = 0;
      for (vcol=0L,mrow=0L ; vcol<numels ; vcol++,mrow++) {
	 vr[mcol] += v[vcol] * (m[mrow])[mcol];
      } /* for */
   } /* for */
   return vr;

} /* MultDVectorMatrix */


DOUBLE *MultDMatrixVector(m, v, vr, numrows, numels)
   DOUBLE *v, **m, *vr;
   LONG numels, numrows; /* Elements in vector, rows in matrix */
{
   LONG mrow, mcol, vrow;
   LONG numcols;

   /* This function calculates m*v, thus v MUST be a COLUMN VECTOR */

   numcols = numels;  /* This MUST be true for multiplication */

   ChkDVector(vr,numrows);
   for (mrow=0L ; mrow<numrows ; mrow++) {
      /* Initialize in case vr has non-zero elements */
      vr[mrow] = 0;
      for (vrow=0L,mcol=0L ; vrow<numels ; vrow++,mcol++) {
	 vr[mrow] += (m[mrow])[mcol] * v[vrow];
      } /* for */
   } /* for */
   return vr;

} /* MultDMatrixVector */


DOUBLE **TransposeDMatrix(DOUBLE **m, DOUBLE **mt, LONG numrows, LONG numcols)
   /*	 m is the matrix to be transposed
	 mt is the transposed matrix
	 numrows is number of rows in m
	 numcols is number of columns in m
   */
{
   LONG i, j;

   /* If mt is NULL, client wants us to allocate.
      Notice that the number of rows and columns are reversed.
   */
   ChkDMatrix(mt,numcols,numrows);

   /* Transpose the matrix */
   for (i=0 ; i<numrows ; i++) {
      for (j=0 ; j<numcols ; j++) {
	 (mt[j])[i] = (m[i])[j];
      }
   }
   return mt;

}  /* TransposeDMatrix */

/*----------------------------------------------------------------------
   The next TWO routines are support routines for InvertDMatrix which
   follows.
------------------------------------------------------------------------*/

/*    LUDecomp() breaks a matrix down into its lower and upper
      triangular parts.
*/
LONG LUDecomp(DOUBLE **a, LONG n, LONG *index)
   /*	 a is the matrix to decompose
	 n is the number of elements in the rows and columns
	 index is an array for storing row permutations
   */
{
   LONG i, imax, j, k;
   DOUBLE biggest, dummy, sum, temp;
   DOUBLE *rowscale;

   rowscale = AllocDVector(n);
   if (!rowscale) return FALSE;  /* Allocation failed */

   for (i=0 ; i<n ; i++) {
      biggest = 0.0;
      for (j=0 ; j<n ; j++) {
	 if ((temp=fabs((a[i])[j])) > biggest)  biggest=temp;
      } /* for	*/
      if (biggest == 0.0)  return FALSE;  /* Singular matrix */
      rowscale[i] = 1.0/biggest;    /* Store the scale factor */
   }  /* for */

   for (j=0 ; j<n ; j++) {
      for (i=0 ; i<j ; i++) {
	 sum = (a[i])[j];
	 for (k=0 ; k<i ; k++)   sum -= (a[i])[k]*(a[k])[j];
	 (a[i])[j] = sum;
      }  /* for */
      biggest = 0.0;
      for (i=j; i<n ; i++) {
	 sum = (a[i])[j];
	 for (k=0 ; k<j ; k++)   sum -= (a[i])[k]*(a[k])[j];
	 (a[i])[j] = sum;
	 if ((dummy=rowscale[i]*fabs(sum)) >= biggest) {
	    biggest = dummy;
	    imax = i;
	 }  /* if */
      }  /* for */
      if (j != imax) {
	 for (k=0 ; k<n ; k++) {
	    dummy = (a[imax])[k];
	    (a[imax])[k] = (a[j])[k];
	    (a[j])[k] = dummy;
	 }  /* for */
	 dummy = rowscale[imax];
	 rowscale[imax]=rowscale[j];
	 rowscale[j] = dummy;
      }  /* if */
      index[j] = imax;
      if ((a[j])[j] == 0.0)   return FALSE;  /* Singular matrix */
      if (j != (n-1)) {
	 dummy = 1.0/(a[j])[j];
	 for (i=j+1 ; i<n ; i++)    (a[i])[j] *= dummy;
      }  /* if */
   }  /* for */
   FreeDVector(rowscale, n);
   return TRUE;
}  /* LUDecomp */


/*    LUFwdBckSub() performs a forward and back substitution on an
      LU decomposed matrix.
*/
void LUFwdBckSub(DOUBLE **a, LONG n, LONG *index, DOUBLE *b)
   /*	 a is the LU decomposed matrix
	 n is the number of elements in the rows and columns
	 index is the row permutation array from LUDecomp()
	 b is the RHS vector
   */
{
   LONG i, nonzero = -1, ip, j;
   DOUBLE sum;

   for (i=0 ; i<n ; i++) {
      ip = index[i];
      sum = b[ip];
      b[ip] = b[i];
      if (nonzero+1) {
	 for (j=nonzero ; j<=i-1 ; j++)   sum -= (a[i])[j]*b[j];
      }
      else if (sum) {
	 nonzero = i;
      }
      b[i] = sum;
   }  /* for */

   for (i=n-1 ; i>=0 ; i--) {
      sum = b[i];
      for (j=i+1 ; j<n ; j++)    sum -= (a[i])[j]*b[j];
      b[i] = sum/(a[i])[i];
   }  /* for */
}  /* LUFwdBckSub */


/*----------------------------------------------------------------
   Here is the InvertDMatrix function
------------------------------------------------------------------*/

DOUBLE **InvertDMatrix(DOUBLE **m, DOUBLE **mcopy, DOUBLE **mi, LONG numrows)
   /*	 m is the matrix to invert
	 mcopy is a copy of m, it will be destroyed in the LU decomposition
	 mi is the inverse matrix
	 numrows is the number of rows and columns in m
   */
{
   DOUBLE *column=NULL;
   LONG i,j, *index=NULL;
   BYTE mcopyflag=FALSE,
	miflag=FALSE,
	success=FALSE;

   /* If mcopy is NULL, client want us to allocate */
   if(!mcopy) {
      mcopy = AllocDMatrix(numrows, numrows);
      if (!mcopy)    return NULL;   /* Allocation failed, abort */
      for (i=0 ; i<numrows ; i++) {
	 for (j=0 ; j<numrows ; j++) {
	    (mcopy[i])[j] = (m[i])[j];
	 }
      }
      mcopyflag = TRUE;    /* Will be used later for deallocation */
   }  /* if */

   /* Allocate index */
   index = AllocLVector(numrows);
   if (!index)    goto ExitInvert;     /* Allocation failed, abort */

   /* Allocate column */
   column = AllocDVector(numrows);
   if (!column)    goto ExitInvert;     /* Allocation failed, abort */

   /* If mi is NULL, client wants us to allocate */
   if (!mi) {
      mi = AllocDMatrix(numrows, numrows);
      if (!mi)    goto ExitInvert;   /* Allocation failed, abort */
      miflag = TRUE;	/* Used later for deallocation */
   }  /* if */

   /* Perform LU decomposition on mcopy */
   i = LUDecomp(mcopy, numrows, index);
   if (!i)   goto ExitInvert;  /* Matrix singular or alloc failed, abort */

   /* Calculate inverse one column at a time */
   for (j=0 ; j<numrows ; j++) {
      for (i=0 ; i<numrows ; i++)   column[i] = 0.0;
      column[j] = 1.0;
      LUFwdBckSub(mcopy, numrows, index, column);
      for (i=0; i<numrows ; i++)    (mi[i])[j] = column[i];
   }  /* for */

   success = TRUE;

ExitInvert:
   if (column)       FreeDVector(column, numrows);
   if (index)        FreeLVector(index, numrows);
   if (mcopyflag)    FreeDMatrix(mcopy, numrows, numrows);
   if (success) {
      return (mi);
   }
   else {
      /* deallocate mi matrix if we created it since routine failed */
      if (miflag)    FreeDMatrix(mi, numrows, numrows);
      return NULL;
   }
}  /* InvertDMatrix */