
/**********************************************************
 *
 * Copyright (c) 1991, John Forkosh.  All rights reserved.
 * Released to the public domain only for use that is both
 * (a) by an individual, and (b) not for profit.
 * --------------------------------------------------------
 *
 * Function:    simq ( a, b, n )
 *
 * Purpose:     Solves the set of n simultaneous linear
 *              equations ax=b.
 *
 * Arguments:
 *              a (I)   Address of n-by-n array of doubles
 *                      stored columnwise (see notes)
 *                      containing the matrix of
 *                      coefficients (which are destroyed
 *                      during the computation).
 *              b (I/O) Address of vector of n doubles,
 *                      containing the original constants,
 *                      and returning the final solution.
 *              n (I)   Int containing the number of
 *                      equations to be solved (must be >1)
 *
 * Returns:     (int)   0 for a normal solution, or
 *                      1 for a singular set of equations.
 *
 * Notes:     o Simq() is derived from the Fortran routine
 *              of the same name in IBM's System/360
 *              Scientific Subroutine Package, Publication
 *              GH20-0205-4, Fifth Edition (August 1970),
 *              Page 120.  See the discussion of Gauss-
 *              Jordan elimination in Chapter 2 of 
 *              Numerical Recipes in C for a similar
 *              treatment.
 *            o The matrix of coefficients, a[], must be
 *              stored columnwise in a singly-dimensioned
 *              array, e.g., for a 3x3 matrix the nine
 *              elements of a[] are
 *                      a[0]  a[3]  a[6]
 *                      a[1]  a[4]  a[7]
 *                      a[2]  a[5]  a[8]
 *              In other words, the index for row i and
 *              column j (i,j=0,...,n-1) is a[n*j+i].
 *              Note that many index calculations are
 *              buried in initialization steps of loops.
 *            o The method of solution is by elimination
 *              using largest pivotal divisor.  Each stage
 *              of elimination consists of interchanging
 *              rows when necessary to avoid division by
 *              zero or small elements.  First the forward
 *              solution to obtain the n-th variable is
 *              done in n stages.  Then the back solution
 *              for the other variables is done by
 *              successive substitution.  Final solution
 *              values are returned in vector b[], with
 *              variable 1 in b[0], variable 2 in b[1],
 *              etc.  If no pivot can be found exceeding
 *              the #defined TOLerance, the matrix is
 *              considered singular and an error returned.
 *
 * Source:      SIMQ.C
 *
 * --------------------------------------------------------
 * Revision History:
 *
 * 01/07/91     J.Forkosh       Installation
 *
 *********************************************************/

/* --- standard headers --- */
#include <stdio.h>
#define dabs(x) ((x)>=0.0?(x):(-(x))) /* absolute value */

/* --- user-adjustable tolerance --- */
#define TOL 0.0

/* --- set testdrive to compile (or not) test main() --- */
#define TESTDRIVE 0             /* 1=compile it (0=not) */

/* --------------------------------------------------------
Entry point
-------------------------------------------------------- */
int     simq ( a, b, n )        /* returns 0=ok, 1=error */
double  *a;                     /* coefficient matrix */
double  *b;                     /* constant vector */
int     n;                      /* number of equations */
{
/* --- local allocations and declarations --- */
int     i,j, ix,jx;             /* row,col indexes */
int     it,k;                   /* temp indexes */

/* --------------------------------------------------------
Forward Solution
-------------------------------------------------------- */
for ( j=0; j<n; j++ )           /* cols */
  {
  /* --- local allocations and declarations --- */
  int   imax;                   /* pivot row */
  double biga=0.0;              /*largest element in col*/
  double save;                  /*save value during pivot*/

  /* ------------------------------------------------------
  Find maximum coefficient (pivot row) in column
  ------------------------------------------------------ */
  for ( it=j*n,i=j; i<n; i++ )  /*rows in lower diagonal*/
    if ( dabs(a[it+i]) > dabs(biga) ) /* got bigger biga */
      { biga = a[it+i];         /* so store biggest */
        imax = i; }             /* and its col offset */
  /* --- error if pivot less than tolerance --- */
  if ( dabs(biga) <= TOL )      /* pivot too small so... */
    return ( 1 );               /*back to caller with err*/

  /* ------------------------------------------------------
  Interchange rows as necessary and divide by leading coeff
  ------------------------------------------------------ */
  /* --- easy to interchange constant vector --- */
  save = b[imax];               /* save b[imax] */
  b[imax] = b[j];               /* replace it with b[j] */
  b[j] = save/biga;             /* swap and rescale */
  /* --- must interchange row one col at a time --- */
  for ( i=j*(n+1),it=imax-j,k=j; k<n; k++,i+=n )
    {
    save = a[it+i];             /* save swap value */
    a[it+i] = a[i];             /* replace it with a[i] */
    a[i] = save/biga;           /* swap and rescale */
    } /* --- end-of-for(k=j...n-1) --- */

  /* ------------------------------------------------------
  Eliminate next variable (Note: the index calculations
  required beyond this point get much more complicated.)
  ------------------------------------------------------ */
  if ( j < (n-1) )              /* except on last j */
   for ( ix=j+1; ix<n; ix++ )   /* lower diagonal */
    {
    int ixj = (n*j)+ix;         /*lowr diag rows in col j*/
    for( it=j-ix,jx=j+1; jx<n; jx++ )
      { int ixjx = (n*jx)+ix;
        a[ixjx] -= a[ixj]*a[it+ixjx]; }
    b[ix] -= b[j]*a[ixj];
    } /* --- end-of-for(ix=j+1...n-1) --- */
  } /* --- end-of-for(j=0...n-1) --- */

/* --------------------------------------------------------
Back Solution
-------------------------------------------------------- */
for ( it=n*n,i=2; i<=n; i++ )   /* all rows except last */
  {
  int   ia=it-i, ib=n-i, ic=n-1;
  for ( k=1; k<i; k++,ia-=n,ic-- )
    b[ib] -= a[ia]*b[ic];
  } /* --- end-of-for(i=2...n) --- */
return ( 0 );                   /*back to caller with b[]*/
} /* --- end-of-function simq() --- */


#if TESTDRIVE
/**********************************************************
 *
 * Purpose:     Test driver for simq().
 *              Solves a set of simultaneous equations
 *              of the form ax=b.
 *
 * Arguments:
 *              N (I)   Int containing the number of
 *                      simultaneous equations to be
 *                      solved (must be <=8 for printf
 *                      output to fit on 80-col screen).
 *              a (I)   Double array containing matrix
 *                      of NxN coefficients.  Note that
 *                      a[] is singly subscripted.
 *                      Values are initialized rowwise
 *                      for easy reading, and transposed
 *                      before calling simq() which
 *                      requires columnwise input.
 *              b (I)   Double array containing vector
 *                      of constants.
 *
 * Notes:     o All input is supplied through the
 *              statements immediately below.  Since
 *              this is only a test/demo of simq(),
 *              no real user interface is supplied.
 *
 *********************************************************/

/* --------------------------------------------------------
Input data to test simq()
-------------------------------------------------------- */
/* --- The user may change the N,a[],b[] test data
       to solve any set of simultaneous equations
       (but don't try more than 8x8 or the results
       won't printf nicely on an 80-column screen). --- */
#define N 7                     /* N-by-N test data */
/* --- Note: a[] is entered rowwise for easy reading,
       and then transposed before calling simq(). --- */
static double a[] =             /*matrix of coefficients*/
 { 1.0, 2.0, 0.0, 0.0, 0.0, 0.0, 0.0,
   0.0, 1.0, 2.0, 0.0, 0.0, 0.0, 0.0,
   1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
   0.0, 0.0, 1.0, 1.0, 1.0, 0.0, 0.0,
   0.0, 0.0, 0.0, 2.0, 1.0, 0.0, 0.0,
   0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0,
   0.0, 0.0, 0.0, 0.0, 0.0, 2.0, 1.0,
 };
static double b[] =             /* vector of constants */
 { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0
 };

/* --------------------------------------------------------
Entry point
-------------------------------------------------------- */
main ( argc, argv )
int argc; char *argv[];         /* args not used */
{
/* --- local allocations and declarations --- */
double  ahold[N*N], bhold[N];   /* copy of original data */
int     i,j, n=N;               /* loop indexes and n */

/* --- copy input arrays for display later --- */
for ( i=0; i<n; i++ )           /*row# (before transpose)*/
  { int k=n*i;                  /* 1st col of row */
    bhold[i] = b[i];            /* copy constant vector */
    for ( j=0; j<n; j++ )       /* each col of row */
        ahold[k+j] = a[k+j]; }  /*copy coefficient matrix*/

/* --- transpose a[] for columnwise input to simq() --- */
for ( i=1; i<n; i++ )           /* each row except 1st */
  for ( j=0; j<i; j++ )         /* upper diagonal */
    {
    int ij = n*i + j;           /* i,j-th element */
    int ji = n*j + i;           /* and its transpose */
    double swap = a[ij];        /* save ... */
    a[ij] = a[ji];  a[ji] = swap; /* ... and swap */
    } /* --- end-of-for(i,j) --- */

/* --- call simq to solve --- */
if ( simq(a,b,n) )              /* error if non-0 return */
  { printf("simq() returned error\n");
    exit(0); }

/* --- output results --- */
printf("Simq() test results (Ax=b)...\n");
for ( i=0; i<n; i++ )           /* each row */
  {
  /* --- print original coefficients (A) for row --- */
  for ( j=0; j<n; j++ ) printf("%6.2lf",ahold[n*i+j]);
  /* --- print result (x) and constant for row (b) --- */
  printf("  %8.2lf   %8.2lf\n", b[i],bhold[i]);
  } /* --- end-of-for(i) --- */
} /* --- end-of-main() --- */
#endif
