Listing 4: guasslup.c

// required headers
#include "gausslup.h"

//
// given a matrix, generate an LUP decomposition using Gaussian
// elimination with scaling and pivoting.
//
template <class T>
int GaussianLUP_Pivot(Matrix<T> &m, Vector<int> &p, T ep, T &sign)
{
    // must be a square matrix
    MustBeTrue(m.getRows() == m.getCols() && m.getRows() > 0);
    sign = -1;

    // check epsilon, set if invalid
    T minep = calcEpsilon(T(0));
    if ((ep = fabs(ep)) < minep)
        ep = minep;

    // get number of rows and columns
    int max = m.getRows();

    // generate scaling information for each row. initialize 
    // permutation array, p.
    int i;
    Vector<T> s(max);
    for (i = 0; i < max; i++)
    {
        p[i] = i;
        if (1 < max)
            s[i] = fabs(m(i, 1));
        else
            s[i] = fabs(m(i, 0));
        for (int j = 2; j < max; j++)
        {
            T tmp = fabs(m(i, j));
            if (tmp > s[i])
                s[i] = tmp;
        }
    }

    // start gaussian elimination process
    for (int k = 0; k < (max-1); k++)
    {
        // find pivot row
        int pivot = k;
        T tmpf = fabs(m(p[pivot], k))/s[p[pivot]];
        for (i = k+1; i < max; i++)
        {
            T tmpf2 = fabs(m(p[i], k))/s[p[i]];
            if (tmpf2 > tmpf)
            {
                pivot = i;
                tmpf = tmpf2;
            }
        }
        if (pivot != k)
        {
            int tmpp = p[k];
            p[k] = p[pivot];
            p[pivot] = tmpp;
            sign = -sign;
        }

        // check for division by zero
        if (fabs(m(p[k], k)) <= ep)
            return(NOTOK);

        // calculate L and U matrices
        for (i = k+1; i < max; i++)
        {
            // multiplier for column
            T d = m(p[i], k)/m(p[k], k);

            // save multiplier since it is L.
            m(p[i], k) = d;

            // reduce original matrix to get U.
            for (int j = k+1; j < max; j++)
            {
                m(p[i], j) -= d*m(p[k], j);
            }
        }
    }

    // all done
    return(OK);
}

//
// given a gaussian LUP decomposition of a matrix and a
// permutation vector, solve for x-vector using the given y-vector
//
template <class T>
int
SolveUsingGaussianLUP_Pivot(Matrix<T> &m, 
    Vector<T> &x, Vector<T> &y, Vector<int> &p, T ep)
{
    // must be a square matrix
    MustBeTrue(m.getRows() == m.getCols() && m.getRows() > 0);

    // check epsilon, set if invalid
    T minep = calcEpsilon(T(0));
    if ((ep = fabs(ep)) < minep)
        ep = minep;

    // get number of rows and columns
    int max = m.getRows();

    // update y-vector
    for (int k = 0; k < (max-1); k++)
    {
        for (int i = k+1; i < max; i++)
        {
            y[p[i]] -= m(p[i], k)*y[p[k]];
        }
    }

    // start backward substitution
    for (int i = max-1; i >= 0; i--)
    {
        // check for a singular matrix
        if (fabs(m(p[i], i)) <= ep)
            return(NOTOK);

        // solve for x by substituting previous solutions
        x[i] = y[p[i]];
        for (int j = i+1; j < max; j++)
        {
            x[i] -= m(p[i], j)*x[j];
        }
        x[i] /= m(p[i], i);
    }

    // all done
    return(OK);
}

//
// given a gaussian LUP decomposition of a matrix and a
// permutation vector, calculate the inverse of the
// original matrix.
//
template <class T>
int
GetInverseUsingGaussianLUP_Pivot(Matrix<T> &m, Matrix<T> &minv,
    Vector<int> &p, T ep)
{
    // must be a square matrix
    MustBeTrue(m.getRows() == m.getCols() && m.getRows() > 0);
    MustBeTrue(minv.getRows() ==
        minv.getCols() && minv.getRows() > 0);
    MustBeTrue(minv.getRows() == m.getCols());

    // get number of rows and columns
    int max = m.getRows();

    // update inverse matrix
    for (int i = 0; i < max; i++)
    {
        // initialize column vector
        Vector<T> y(max);
        int j;
        for (j = 0; j < max; j++)
        {
            y[j] = 0;
        }
        y[i] = 1;

        // solve for corresponding vector in inverse
        Vector<T> x(max);
        if (SolveUsingGaussianLUP_Pivot(m, x, y, p, ep) != OK)
            return(NOTOK);

        // transfer results to inverse matrix
        for (j = 0; j < max; j++)
        {
            minv(j, i) = x[j];
        }
    }

    // all done
    return(OK);
}
//End of File