/*
    This file is part of the FElt finite element analysis package.
    Copyright (C) 1993 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:	matrix.c						*
 *									*
 * Description:	This file contains the function definitions for the	*
 *		matrix package.						*
 ************************************************************************/

# include <stdio.h>
# include <math.h>
# include "matrix.h"
# include "allocate.h"

/************************************************************************
 * Function:	 LUPDecomposition					*
 *									*
 * Parameters:	 a  - destination matrix				*
 *		 pi - permutation vector				*
 *									*
 * Return value: 0 on error; 1 if number of interchanges is even; -1 if	*
 *		 number of interchanges is odd				*
 *									*
 * Calls:	 none							*
 *									*
 * Called by:	 LUDecomposition					*
 *									*
 * Global data:	 none							*
 *									*
 * Description:	 LUPDecomposition performs the LU decomposition of the	*
 *		 specified matrix permuting the rows of the matrix as	*
 *		 needed.  The code is from "Introduction to Algorithms"	*
 *		 by Cormen, Leiserson, and Rivest p. 759.		*
 ************************************************************************/

static int LUPDecomposition (a, pi)
    Matrix a;
    Vector pi;
{
    double   p, temp, v, scale [100];
    unsigned i, j, k, kp, n;

    n = MatrixRows (a);

    for (i = 1; i <= n; i ++) {
	p = 0;
	for (j = 1; j <= n; j ++) {
	    v = fabs (MatrixData (a) [i] [j]);
	    if (v > p)
		p = v;
	}

	if (p == 0)
	    return 0;

	scale [i] = p;
    }

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

    for (k = 1; k <= n; k ++) {
	p = 0;
	kp = 0;
	for (i = k; i <= n; i ++) {
	    if (scale [i] > 0) {
	    v = fabs (MatrixData (a) [i] [k]) / scale [i];
	    if (v > p) {
		p = v;
		kp = i;
	    }
	    }
	}

	if (kp == 0)  {
	    fprintf (stderr, "k = %d  n = %d\n", k, n);
	    return 0;
	}

	if (kp != k) {
	temp = VectorData (pi) [k];
	VectorData (pi) [k] = VectorData (pi) [kp];
	VectorData (pi) [kp] = temp;

	for (i = 1; i <= n; i ++) {
	    temp = MatrixData (a) [k] [i];
	    MatrixData (a) [k] [i] = MatrixData (a) [kp] [i];
	    MatrixData (a) [kp] [i] = temp;
	}
	}

	for (i = k + 1; i <= n; i ++) {
	    MatrixData (a) [i] [k] /= MatrixData (a) [k] [k];
	    for (j = k + 1; j <= n; j ++)
		MatrixData (a) [i] [j] -= MatrixData (a) [i] [k] *
					  MatrixData (a) [k] [j];
	}
    }
	
    return 1;
}

/************************************************************************
 * Function:	 CreateMatrix						*
 *									*
 * Parameters:	 nrows - number of rows					*
 *		 ncols - number of columns				*
 *									*
 * Return value: a new matrix on success; a NullMatrix on error		*
 *									*
 * Calls:	 CreateSubmatrix					*
 *									*
 * Called by:	 CreateVector, public					*
 *									*
 * Global data:	 none							*
 *									*
 * Description:	 CreateMatrix creates and returns a new matrix of the	*
 *		 specified size.  The matrix has both pointer and data	*
 *		 memory allocated.  If a new matrix cannot be created	*
 *		 then a NullMatrix is returned.				*
 ************************************************************************/

Matrix CreateMatrix (nrows, ncols)
    unsigned nrows;
    unsigned ncols;
{
    Matrix   m;
    unsigned i;

    if (!(m = CreateSubmatrix (nrows, ncols)))
	return NullMatrix;

    if (!(MatrixData (m) [1] = Allocate (double, nrows * ncols))) {
	DestroySubmatrix (m);
	return NullMatrix;
    }

    UnitOffset (MatrixData (m) [1]);
    for (i = 2; i <= nrows; i ++)
	MatrixData (m) [i] = MatrixData (m) [i - 1] + ncols;

    VectorData (m) = MatrixData (m) [1];
    return m;
}


/************************************************************************
 * Function:	 CreateSubmatrix					*
 *									*
 * Parameters:	 nrows - number of rows					*
 *		 ncols - number of columns				*
 *									*
 * Return value: a new submatrix on success; a NullMatrix on error	*
 *									*
 * Calls:	 none							*
 *									*
 * Called by:	 CreateMatrix, public					*
 *									*
 * Global data:	 none							*
 *									*
 * Description:	 CreateSubmatrix creates and returns a new submatrix of	*
 *		 the specified size.  The submatrix has only pointer	*
 *		 memory allocated.  If a new submatrix cannot be	*
 *		 created then a NullMatrix is returned.			*
 ************************************************************************/

Matrix CreateSubmatrix (nrows, ncols)
    unsigned nrows;
    unsigned ncols;
{
    Matrix m;

    if (!nrows || !ncols || !(m = AllocNew (struct matrix)))
	return NullMatrix;

    if (!(MatrixData (m) = Allocate (double *, nrows))) {
	Deallocate (m);
	return NullMatrix;
    }

    UnitOffset (MatrixData (m));
    MatrixRows (m) = nrows;
    MatrixCols (m) = ncols;
    return m;
}


/************************************************************************
 * Function:	 CreateVector						*
 *									*
 * Parameters:	 size - vector size					*
 *									*
 * Return value: a new vector on success; a NullVector on error		*
 *									*
 * Calls:	 CreateMatrix						*
 *									*
 * Called by:	 public							*
 *									*
 * Global data:	 none							*
 *									*
 * Description:	 CreateVector creates and returns a new vector which	*
 *		 is simply a matrix with one column.  If a new vector	*
 *		 cannot be created then a NullVector is returned.	*
 ************************************************************************/

Vector CreateVector (size)
    unsigned size;
{
    return CreateMatrix (size, 1);
}


/************************************************************************
 * Function:	 DestroyMatrix						*
 *									*
 * Parameters:	 m - matrix to destroy					*
 *									*
 * Return value: none							*
 *									*
 * Calls:	 DestroySubmatrix					*
 *									*
 * Called by:	 DestroyVector, public					*
 *									*
 * Global data:	 none							*
 *									*
 * Description:	 DestroyMatrix destroys the specified matrix.  Both the	*
 *		 the data and pointer memory are deallocated.		*
 ************************************************************************/

void DestroyMatrix (m)
    Matrix m;
{
    ZeroOffset (MatrixData (m) [1]);
    Deallocate (MatrixData (m) [1]);
    DestroySubmatrix (m);
}


/************************************************************************
 * Function:	 DestroySubmatrix					*
 *									*
 * Parameters:	 m - submatrix to destroy				*
 *									*
 * Return value: none							*
 *									*
 * Calls:	 none							*
 *									*
 * Called by:	 DestroyMatrix, public					*
 *									*
 * Global data:	 none							*
 *									*
 * Description:	 DestroySubmatrix destroys the specified submatrix.	*
 *		 Only the pointer memory is deallocated.		*
 ************************************************************************/

void DestroySubmatrix (m)
     Matrix m;
{
    ZeroOffset (MatrixData (m));
    Deallocate (MatrixData (m));
    Deallocate (m);
}


/************************************************************************
 * Function:	 DestroyVector						*
 *									*
 * Parameters:	 v - vector to destroy					*
 *									*
 * Return value: none							*
 *									*
 * Calls:	 DestroyMatrix						*
 *									*
 * Called by:	 public							*
 *									*
 * Global data:	 none							*
 *									*
 * Description:  DestroyVector destroys the specified vector.		*
 ************************************************************************/

void DestroyVector (v)
    Vector v;
{
    DestroyMatrix (v);
}


/************************************************************************
 * Function:	 PrintMatrix						*
 *									*
 * Parameters:	 m - matrix to print					*
 *		 f - stream pointer					*
 *									*
 * Return value: none							*
 *									*
 * Calls:	 none							*
 *									*
 * Called by:	 PrintVector						*
 *									*
 * Global data:	 none							*
 *									*
 * Description:	 PrintMatrix prints the specified matrix to the		*
 *		 specified stream.  If a row of the matrix is too long	*
 *		 to fit on an 80 column screen then the row is		*
 *		 continued on the next line with indentation.		*
 ************************************************************************/

void PrintMatrix (m, f)
    Matrix m;
    FILE  *f;
{
    unsigned i, j;

    if (m == NullMatrix) {
	fprintf (f, "(null)\n\n");
	return;
    }

    for (i = 1; i <= MatrixRows (m); i ++) {
	for (j = 1; j <= MatrixCols (m); j ++) {
	    fprintf (f, "%10g ", MatrixData (m) [i] [j]);
	    if (j % 7 == 0)
		fprintf (f, "\n> ");
	}
	fprintf (f, "\n");
    }

    fprintf (f, "\n");
}


/************************************************************************
 * Function:	 PrintVector						*
 *									*
 * Parameters:	 v - vector to print					*
 *		 f - stream pointer					*
 *									*
 * Return value: none							*
 *									*
 * Calls:	 PrintMatrix						*
 *									*
 * Called by:	 public							*
 *									*
 * Global data:	 none							*
 *									*
 * Description:	 PrintVector prints the specified vector to the		*
 *		 specified stream.					*
 ************************************************************************/

void PrintVector (v, f)
    Vector v;
    FILE  *f;
{
    PrintMatrix (v, f);
}


/************************************************************************
 * Function:	 SetSubmatrix						*
 *									*
 * Parameters:	 s   - submatrix					*
 *		 m   - parent matrix					*
 *		 row - starting row					*
 *		 col - starting column					*
 *									*
 * Return value: specified submatrix on success; a NullMatrix on error	*
 *									*
 * Calls:	 none							*
 *									*
 * Called by:	 public							*
 *									*
 * Global data:	 none							*
 *									*
 * Description:	 SetSubmatrix sets a submatrix to be the data starting	*
 *		 at the specified row and column of the parent matrix.	*
 *		 The data of the submatrix is shared with the data of	*
 *		 the parent matrix and the submatrix is returned.  If	*
 *		 the submatrix is not contained within the parent	*
 *		 matrix, a NullMatrix is returned.			*
 ************************************************************************/

Matrix SetSubmatrix (s, m, row, col)
    Matrix   s;
    Matrix   m;
    unsigned row;
    unsigned col;
{
    unsigned i;

    if (!row || !col || s == NullMatrix || m == NullMatrix)
	return NullMatrix;

    if (row + MatrixRows (s) > MatrixRows (m) + 1)
	return NullMatrix;

    if (col + MatrixCols (s) > MatrixCols (m) + 1)
	return NullMatrix;

    for (i = 1; i <= MatrixRows (s); i ++)
	MatrixData (s) [i] = MatrixData (m) [i + row - 1] + col - 1;

    return s;
}


/************************************************************************
 * Function:	 ScaleMatrix						*
 *									*
 * Parameters:	 a	- destination matrix				*
 *		 b	- source matrix					*
 *		 factor - multiplicative factor				*
 *		 offset - constant offset				*
 *									*
 * Return value: result matrix on success; a NullMatrix on error	*
 *									*
 * Calls:	 CreateMatrix						*
 *									*
 * Called by:	 public							*
 *									*
 * Global data:	 none							*
 *									*
 * Description:	 ScaleMatrix scales the source matrix by the specified	*
 *		 factor and offset and the result is returned.  If the	*
 *		 destination matrix is not a NullMatrix then it will	*
 *		 contain the result.  Otherwise a new matrix is created	*
 *		 for the result.  If the scaling cannot be performed	*
 *		 then a NullMatrix is returned.				*
 ************************************************************************/

Matrix ScaleMatrix (a, b, factor, offset)
    Matrix a;
    Matrix b;
    double factor;
    double offset;
{
    unsigned i, j;

    if (b == NullMatrix)
	return NullMatrix;

    if (a == NullMatrix)
	if (!(a = CreateMatrix (MatrixRows (b), MatrixCols (b))))
	    return NullMatrix;

    if (MatrixRows (a) != MatrixRows (b) || MatrixCols (a) != MatrixCols (b))
	return NullMatrix;

    for (i = 1; i <= MatrixRows (a); i ++)
	for (j = 1; j <= MatrixCols (a); j ++)
	    MatrixData (a) [i] [j] = MatrixData (b) [i] [j] * factor + offset;

    return a;
}


/************************************************************************
 * Function:	 AssignMatrix						*
 *									*
 * Parameters:	 a - destination matrix					*
 *		 b - source matrix					*
 *									*
 * Return value: result matrix on success; a NullMatrix on error	*
 *									*
 * Calls:	 none							*
 *									*
 * Called by:	 public							*
 *									*
 * Global data:	 none							*
 *									*
 * Description:	 AssignMatrix assigns the source matrix to the		*
 *		 destination matrix and returns the destination matrix.	*
 *		 If the destination matrix is a NullMatrix then a new	*
 *		 matrix is created as the destination matrix.  If the	*
 *		 assignment cannot be made then a NullMatrix is		*
 *		 returned.						*
 ************************************************************************/

Matrix AssignMatrix (a, b)
    Matrix a;
    Matrix b;
{
    unsigned i, j;

    if (b == NullMatrix)
	return NullMatrix;

    if (a == NullMatrix)
	if (!(a = CreateMatrix (MatrixRows (b), MatrixCols (b))))
	    return NullMatrix;

    if (MatrixRows (a) != MatrixRows (b) || MatrixCols (a) != MatrixCols (b))
	return NullMatrix;

    for (i = 1; i <= MatrixRows (a); i ++)
	for (j = 1; j <= MatrixCols (a); j ++)
	    MatrixData (a) [i] [j] = MatrixData (b) [i] [j];

    return a;
}


/************************************************************************
 * Function:	 InvertMatrix						*
 *									*
 * Parameters:	 a - destination matrix					*
 *		 b - source matrix					*
 *									*
 * Return value: result matrix on success; a NullMatrix on error	*
 *									*
 * Calls:	 CreateVector, DestroyMatrix, DestroyVector,		*
 *		 LUDecomposition, LUBackSolve				*
 *									*
 * Called by:	 public							*
 *									*
 * Global data:	 none							*
 *									*
 * Description:	 InvertMatrix inverts the specified matrix and returns	*
 *		 the result.  If the destination matrix is not a	*
 *		 NullMatrix then it will contain the result.  Otherwise	*
 *		 a new matrix is created for the result.  If the	*
 *		 inversion cannot be performed then a NullMatrix is	*
 *		 returned.						*
 ************************************************************************/

Matrix InvertMatrix (a, b)
    Matrix a;
    Matrix b;
{
    return NullMatrix;
}


/************************************************************************
 * Function:	 TransposeMatrix					*
 *									*
 * Parameters:	 a - destination matrix					*
 *		 b - source matrix					*
 *									*
 * Return value: result matrix on success; a NullMatrix on error	*
 *									*
 * Calls:	 CreateMatrix						*
 *									*
 * Called by:	 public							*
 *									*
 * Global data:	 none							*
 *									*
 * Description:	 TransposeMatrix transposes the source matrix and	*
 *		 returns the result.  If the destination matrix is not	*
 *		 a NullMatrix then it will contain the result.		*
 *		 Otherwise a new matrix is created for the result.  If	*
 *		 the transposition cannot be performed then a		*
 *		 NullMatrix is returned.				*
 ************************************************************************/

Matrix TransposeMatrix (a, b)
    Matrix a;
    Matrix b;
{
    unsigned i, j;

    if (b == NullMatrix || a == b)
	return NullMatrix;

    if (a == NullMatrix)
	if (!(a = CreateMatrix (MatrixCols (b), MatrixRows (b))))
	    return NullMatrix;

    if (MatrixRows (a) != MatrixCols (b) || MatrixCols (a) != MatrixRows (b))
	return NullMatrix;

    for (i = 1; i <= MatrixRows (a); i ++)
	for (j = 1; j <= MatrixCols (a); j ++)
	    MatrixData (a) [i] [j] = MatrixData (b) [j] [i];

    return a;
}


/************************************************************************
 * Function:	 MirrorMatrix						*
 *									*
 * Parameters:	 a - destination matrix					*
 *		 b - source matrix					*
 *									*
 * Return value: result matrix on success; a NullMatrix on error	*
 *									*
 * Calls:	 CreateMatrix						*
 *									*
 * Called by:	 public							*
 *									*
 * Global data:	 none							*
 *									*
 * Description:	 MirrorMatrix mirrors a matrix by assigning the upper	*
 *		 triangular elements to the lower triangular elements	*
 *		 and returns the result.  If the destination matrix is	*
 *		 not a NullMatrix then it will contain the result.	*
 *		 Otherwise a new matrix is created for the result.  If	*
 *		 the matrix cannot be mirrored then a NullMatrix is	*
 *		 returned.						*
 ************************************************************************/

Matrix MirrorMatrix (a, b)
    Matrix a, b;
{
    unsigned i, j;

    if (b == NullMatrix || MatrixRows (b) != MatrixCols (b))
	return NullMatrix;

    if (a == NullMatrix)
	if (!(a = CreateMatrix (MatrixRows (b), MatrixCols (b))))
	    return NullMatrix;

    if (MatrixRows (a) != MatrixRows (b) || MatrixCols (a) != MatrixCols (b))
	return NullMatrix;

    if (a == b) {
        for (i = 2 ; i <= MatrixRows (a) ; i++)
            for (j = 1 ; j < i ; j++)
                MatrixData (a) [i] [j] = MatrixData (b) [j] [i];
    }
    else { 
        for (i = 1; i <= MatrixRows (a); i ++)
      	    for (j = 1; j <= MatrixCols (a); j ++)
	        if (i <= j)
		    MatrixData (a) [i] [j] = MatrixData (b) [i] [j];
	        else
		    MatrixData (a) [i] [j] = MatrixData (b) [j] [i];
    }

    return a;
}


/************************************************************************
 * Function:	 ZeroMatrix						*
 *									*
 * Parameters:	 a - matrix to clear					*
 *									*
 * Return value: input matrix on success; a NullMatrix on error		*
 *									*
 * Calls:	 none							*
 *									*
 * Called by:	 public							*
 *									*
 * Global data:	 none							*
 *									*
 * Description:	 ZeroMatrix initializes the specified matrix to zero.	*
 *		 The specified matrix is returned.			*
 ************************************************************************/

Matrix ZeroMatrix (a)
    Matrix a;
{
    unsigned i, j;

    if (a == NullMatrix)
	return NullMatrix;

    for (i = 1; i <= MatrixRows (a); i ++)
	for (j = 1; j <= MatrixCols (a); j ++)
	    MatrixData (a) [i] [j] = 0;

    return a;
}


/************************************************************************
 * Function:	 AddMatrices						*
 *									*
 * Parameters:	 a - destination matrix					*
 *		 b - left source matrix					*
 *		 c - right source matrix				*
 *									*
 * Return value: result matrix on success; a NullMatrix on error	*
 *									*
 * Calls:	 CreateMatrix						*
 *									*
 * Called by:	 public							*
 *									*
 * Global data:	 none							*
 *									*
 * Description:	 AddMatrices adds the two source matrices and returns	*
 *		 the result.  If the destination matrix is not a	*
 *		 NullMatrix then it will contain the result.  Otherwise	*
 *		 a new matrix is created to contain the result.  If the	*
 *		 addition cannot be performed then a NullMatrix is	*
 *		 returned.						*
 ************************************************************************/

Matrix AddMatrices (a, b, c)
    Matrix a;
    Matrix b;
    Matrix c;
{
    unsigned i, j;

    if (b == NullMatrix || c == NullMatrix)
	return NullMatrix;

    if (MatrixRows (b) != MatrixRows (c) || MatrixCols (b) != MatrixCols (c))
	return NullMatrix;

    if (a == NullMatrix)
	if (!(a = CreateMatrix (MatrixRows (b), MatrixCols (b))))
	    return NullMatrix;

    if (MatrixRows (a) != MatrixRows (b) || MatrixCols (a) != MatrixCols (b))
	return NullMatrix;

    for (i = 1; i <= MatrixRows (a); i ++)
	for (j = 1; j <= MatrixCols (a); j ++)
	    MatrixData(a) [i][j] = MatrixData(b) [i][j] + MatrixData(c) [i][j];

    return a;
}


/************************************************************************
 * Function:	 SubtractMatrices					*
 *									*
 * Parameters:	 a - destination matrix					*
 *		 b - left source matrix					*
 *		 c - right source matrix				*
 *									*
 * Return value: result matrix on success; a NullMatrix on error	*
 *									*
 * Calls:	 CreateMatrix						*
 *									*
 * Called by:	 public							*
 *									*
 * Global data:	 none							*
 *									*
 * Description:	 SubtractMatrices subtracts the two source matrices and	*
 *		 returns the result.  If the destination matrix is not	*
 *		 a NullMatrix then it will contain the result.		*
 *		 Otherwise a new matrix is created for the result.  If	*
 *		 the subtraction cannot be performed then a NullMatrix	*
 *		 is returned.						*
 ************************************************************************/

Matrix SubtractMatrices (a, b, c)
    Matrix a;
    Matrix b;
    Matrix c;
{
    unsigned i, j;

    if (b == NullMatrix || c == NullMatrix)
	return NullMatrix;

    if (MatrixRows (b) != MatrixRows (c) || MatrixCols (b) != MatrixCols (c))
	return NullMatrix;

    if (a == NullMatrix)
	if (!(a = CreateMatrix (MatrixRows (b), MatrixCols (b))))
	    return NullMatrix;

    if (MatrixRows (a) != MatrixRows (b) || MatrixCols (a) != MatrixCols (b))
	return NullMatrix;

    for (i = 1; i <= MatrixRows (a); i ++)
	for (j = 1; j <= MatrixCols (a); j ++)
	    MatrixData(a) [i][j] = MatrixData(b) [i][j] - MatrixData(c) [i][j];

    return a;
}


/************************************************************************
 * Function:	 MultiplyMatrices					*
 *									*
 * Parameters:	 a - destination matrix					*
 *		 b - left source matrix					*
 *		 c - right source matrix				*
 *									*
 * Return value: result matrix on success; a NullMatrix on error	*
 *									*
 * Calls:	 CreateMatrix						*
 *									*
 * Called by:	 public							*
 *									*
 * Global data:	 none							*
 *									*
 * Description:	 MultiplyMatrices multiplies the two source matrices	*
 *		 and returns the result.  If the destination matrix is	*
 *		 not a NullMatrix then it will contain the result.	*
 *		 Otherwise a new matrix is created for the result.  If	*
 *		 the multiplication cannot be performed then a		*
 *		 NullMatrix is returned.				*
 ************************************************************************/

Matrix MultiplyMatrices (a, b, c)
    Matrix a;
    Matrix b;
    Matrix c;
{
    double   sum;
    unsigned i, j, k;

    if (b == NullMatrix || c == NullMatrix || a == b || a == c)
	return NullMatrix;

    if (MatrixCols (b) != MatrixRows (c))
	return NullMatrix;

    if (a == NullMatrix)
	if (!(a = CreateMatrix (MatrixRows (b), MatrixCols (c))))
	    return NullMatrix;

    if (MatrixRows (a) != MatrixRows (b) || MatrixCols (a) != MatrixCols (c))
	return NullMatrix;

    for (i = 1; i <= MatrixRows (a); i ++)
	for (j = 1; j <= MatrixCols (a); j ++) {
	    sum = 0;
	    for (k = 1; k <= MatrixCols (b); k ++)
		sum += MatrixData (b) [i] [k] * MatrixData (c) [k] [j];
	    MatrixData (a) [i] [j] = sum;
	}

    return a;
}


/************************************************************************
 * Function:	 LUDecomposition					*
 *									*
 * Parameters:	 a - destination matrix for result			*
 *		 p - destination vector for permutation array		*
 *		 b - source matrix					*
 *									*
 * Return value: result matrix on success; a NullMatrix on error	*
 *									*
 * Calls:	 CreateMatrix, CreateVector, DestroyMatrix,		*
 *		 DestroyMatrix, LUPDecomposition			*
 *									*
 * Called by:	 InvertMatrix, public					*
 *									*
 * Global data:	 none							*
 *									*
 * Description:	 LUDecomposition computes the LU decomposition of the	*
 *		 specified matrix and returns the result.  If the	*
 *		 destination matrix is not a NullMatrix then it will	*
 *		 contains the result.  Otherwise a new matrix is	*
 *		 created for the result.				*
 ************************************************************************/

Matrix LUDecomposition (a, p, b)
    Matrix a;
    Vector p;
    Matrix b;
{
    unsigned alocal, i, j, plocal;

    if (b == NullMatrix || MatrixRows (b) != MatrixCols (b))
	return NullMatrix;

    plocal = (p == NullVector);
    if (plocal)
	if (!(p = CreateVector (MatrixRows (b))))
	    return NullMatrix;

    if (VectorSize (p) != MatrixRows (b))
	return NullMatrix;

    alocal = (a == NullMatrix);
    if (alocal)
	if (!(a = CreateMatrix (MatrixRows (b), MatrixCols (b)))) {
	    if (plocal) DestroyVector (p);
	    return NullMatrix;
	}

    if (MatrixRows (a) != MatrixRows (b) || MatrixCols (a) != MatrixCols (b)) {
	if (plocal) DestroyVector (p);
	return NullMatrix;
    }

    if (a != b)
	for (i = 1; i <= MatrixRows (a); i ++)
	    for (j = 1; j <= MatrixCols (a); j ++)
		MatrixData (a) [i] [j] = MatrixData (b) [i] [j];

    if (LUPDecomposition (a, p) == 0) {
	if (alocal) DestroyMatrix (a);
	if (plocal) DestroyVector (p);
	return NullMatrix;
    }

    if (plocal)
	DestroyVector (p);

    return a;
}


/************************************************************************
 * Function:	 LUBackSolve						*
 *									*
 * Parameters:	 x - destination vector					*
 *		 a - LU factorization matrix				*
 *		 p - LU permutation vector				*
 *		 b - right hand source vector				*
 *									*
 * Return Value: result vector on success; a NullVector on error	*
 *									*
 * Calls:	 CreateVector, nr_lubksb				*
 *									*
 * Called by:	 InvertMatrix, public					*
 *									*
 * Global data:	 none							*
 *									*
 * Description:	 LUBackSolve solves the equation Ax=b and returns the	*
 *		 result.  If the destination vector is not a NullVector	*
 *		 then it will contain the result.  Otherwise a new	*
 *		 vector is created for the result.  If the system	*
 *		 cannot cannot be solved then a NullVector is returned.	*
 ************************************************************************/

Vector LUBackSolve (x, a, p, b)
    Vector x;
    Matrix a;
    Vector p;
    Vector b;
{
    double   sigma;
    unsigned i, j, n;

    if (a == NullMatrix || p == NullVector || b == NullVector)
	return NullVector;

    if (MatrixRows (a) != MatrixCols (a))
	return NullVector;

    if (VectorSize (p) != MatrixRows (a) || VectorSize (b) != MatrixRows (a))
	return NullVector;

    if (x == NullVector)
	if (!(x = CreateVector (VectorSize (b))))
	    return NullVector;

    if (VectorSize (x) != VectorSize (b))
	return NullVector;

    if (x != b)
	for (i = 1; i <= VectorSize (b); i ++)
	    VectorData (x) [i] = VectorData (b) [i];


    /* LUPSolve from CLR p. 753 */

    n = MatrixRows (a);

    for (i = 1; i <= n; i ++) {
	sigma = 0;
	for (j = 1; j < i; j ++)
	    sigma += MatrixData (a) [i] [j] * VectorData (x) [j];

	VectorData (x) [i] = VectorData (b) [(int) VectorData (p) [i]] - sigma;
    }

    for (i = n; i >= 1; i --) {
	sigma = 0;
	for (j = i + 1; j <= n; j ++)
	    sigma += MatrixData (a) [i] [j] * VectorData (x) [j];

	VectorData (x) [i] = (VectorData (x) [i]-sigma) / MatrixData (a) [i][i];
    }

    return x;
}
