| 1 | #include "slalib.h"
|
|---|
| 2 | #include "slamac.h"
|
|---|
| 3 | void slaSmat ( int n, float *a, float *y, float *d, int *jf, int *iw )
|
|---|
| 4 | /*
|
|---|
| 5 | ** - - - - - - - -
|
|---|
| 6 | ** s l a S m a t
|
|---|
| 7 | ** - - - - - - - -
|
|---|
| 8 | **
|
|---|
| 9 | ** Matrix inversion & solution of simultaneous equations.
|
|---|
| 10 | **
|
|---|
| 11 | ** (single precision)
|
|---|
| 12 | **
|
|---|
| 13 | ** For the set of n simultaneous equations in n unknowns:
|
|---|
| 14 | ** a.y = x
|
|---|
| 15 | **
|
|---|
| 16 | ** where:
|
|---|
| 17 | ** a is a non-singular n x n matrix
|
|---|
| 18 | ** y is the vector of n unknowns
|
|---|
| 19 | ** x is the known vector
|
|---|
| 20 | **
|
|---|
| 21 | ** slaSmat computes:
|
|---|
| 22 | ** the inverse of matrix a
|
|---|
| 23 | ** the determinant of matrix a
|
|---|
| 24 | ** the vector of n unknowns
|
|---|
| 25 | **
|
|---|
| 26 | ** Arguments:
|
|---|
| 27 | **
|
|---|
| 28 | ** symbol type dimension before after
|
|---|
| 29 | **
|
|---|
| 30 | ** n int no. of unknowns unchanged
|
|---|
| 31 | ** *a float [n][n] matrix inverse
|
|---|
| 32 | ** *y float [n] vector solution
|
|---|
| 33 | ** *d float - determinant
|
|---|
| 34 | ** > *jf int - singularity flag
|
|---|
| 35 | ** *iw int [n] - workspace
|
|---|
| 36 | **
|
|---|
| 37 | **
|
|---|
| 38 | ** > jf is the singularity flag. If the matrix is non-singular,
|
|---|
| 39 | ** jf=0 is returned. If the matrix is singular, jf=-1 & d=0.0 are
|
|---|
| 40 | ** returned. In the latter case, the contents of array a on return
|
|---|
| 41 | ** are undefined.
|
|---|
| 42 | **
|
|---|
| 43 | ** Algorithm:
|
|---|
| 44 | ** Gaussian elimination with partial pivoting.
|
|---|
| 45 | **
|
|---|
| 46 | ** Speed:
|
|---|
| 47 | ** Very fast.
|
|---|
| 48 | **
|
|---|
| 49 | ** Accuracy:
|
|---|
| 50 | ** Fairly accurate - errors 1 to 4 times those of routines optimized
|
|---|
| 51 | ** for accuracy.
|
|---|
| 52 | **
|
|---|
| 53 | ** Example call (note handling of "adjustable dimension" 2D array):
|
|---|
| 54 | **
|
|---|
| 55 | ** float a[MP][MP], v[MP], d;
|
|---|
| 56 | ** int j, iw[MP];
|
|---|
| 57 | ** :
|
|---|
| 58 | ** slaSmat ( n, (float *) a, v, &d, &j, iw );
|
|---|
| 59 | **
|
|---|
| 60 | ** Last revision: 17 August 1999
|
|---|
| 61 | **
|
|---|
| 62 | ** Copyright P.T.Wallace. All rights reserved.
|
|---|
| 63 | */
|
|---|
| 64 |
|
|---|
| 65 | #define TINY 1e-20f
|
|---|
| 66 |
|
|---|
| 67 | {
|
|---|
| 68 | int k, imx, i, j, ki;
|
|---|
| 69 | float amx, t, yk;
|
|---|
| 70 |
|
|---|
| 71 | /* Pointers to beginnings of rows in matrix a[n][n] */
|
|---|
| 72 |
|
|---|
| 73 | float *ak, /* row k */
|
|---|
| 74 | *ai, /* row i */
|
|---|
| 75 | *aimx; /* row imx */
|
|---|
| 76 |
|
|---|
| 77 | *jf = 0;
|
|---|
| 78 | *d = 1.0f;
|
|---|
| 79 |
|
|---|
| 80 | for ( k = 0, ak = a; k < n; k++, ak += n ) {
|
|---|
| 81 | amx = (float) fabs ( (double) ak[k] );
|
|---|
| 82 | imx = k;
|
|---|
| 83 | aimx = ak;
|
|---|
| 84 | if ( k != n ) {
|
|---|
| 85 | for ( i = k + 1, ai = ak + n; i < n; i++, ai += n ) {
|
|---|
| 86 | t = (float) fabs ( (double) ai[k] );
|
|---|
| 87 | if ( t > amx ) {
|
|---|
| 88 | amx = t;
|
|---|
| 89 | imx = i;
|
|---|
| 90 | aimx = ai;
|
|---|
| 91 | }
|
|---|
| 92 | }
|
|---|
| 93 | }
|
|---|
| 94 | if ( amx < TINY ) {
|
|---|
| 95 | *jf = -1;
|
|---|
| 96 | } else {
|
|---|
| 97 | if ( imx != k ) {
|
|---|
| 98 | for ( j = 0; j < n; j++ ) {
|
|---|
| 99 | t = ak[j];
|
|---|
| 100 | ak[j] = aimx[j];
|
|---|
| 101 | aimx[j] = t;
|
|---|
| 102 | }
|
|---|
| 103 | t = y[k];
|
|---|
| 104 | y[k] = y[imx];
|
|---|
| 105 | y[imx] = t;
|
|---|
| 106 | *d = - *d;
|
|---|
| 107 | }
|
|---|
| 108 | iw[k] = imx;
|
|---|
| 109 | *d *= ak[k];
|
|---|
| 110 | if ( fabs ( *d ) < TINY ) {
|
|---|
| 111 | *jf = -1;
|
|---|
| 112 | } else {
|
|---|
| 113 | ak[k] = 1.0f / ak[k];
|
|---|
| 114 | for ( j = 0; j < n; j++ ) {
|
|---|
| 115 | if ( j != k ) {
|
|---|
| 116 | ak[j] *= ak[k];
|
|---|
| 117 | }
|
|---|
| 118 | }
|
|---|
| 119 | yk = y[k] * ak[k];
|
|---|
| 120 | y[k] = yk;
|
|---|
| 121 | for ( i = 0, ai = a; i < n; i++ , ai += n ) {
|
|---|
| 122 | if ( i != k ) {
|
|---|
| 123 | for ( j = 0; j < n; j++ ) {
|
|---|
| 124 | if ( j != k ) {
|
|---|
| 125 | ai[j] -= ai[k] * ak[j];
|
|---|
| 126 | }
|
|---|
| 127 | }
|
|---|
| 128 | y[i] -= ai[k] * yk;
|
|---|
| 129 | }
|
|---|
| 130 | }
|
|---|
| 131 | for ( i = 0, ai = a; i < n; i++ , ai += n ) {
|
|---|
| 132 | if ( i != k )
|
|---|
| 133 | ai[k] *= - ak[k];
|
|---|
| 134 | }
|
|---|
| 135 | }
|
|---|
| 136 | }
|
|---|
| 137 | }
|
|---|
| 138 | if ( *jf != 0 ) {
|
|---|
| 139 | *d = 0.0f;
|
|---|
| 140 | } else {
|
|---|
| 141 | for ( k = n; k-- > 0; ) {
|
|---|
| 142 | ki = iw[k];
|
|---|
| 143 | if ( k != ki ) {
|
|---|
| 144 | for ( i = 0, ai = a; i < n; i++ , ai += n ) {
|
|---|
| 145 | t = ai[k];
|
|---|
| 146 | ai[k] = ai[ki];
|
|---|
| 147 | ai[ki] = t;
|
|---|
| 148 | }
|
|---|
| 149 | }
|
|---|
| 150 | }
|
|---|
| 151 | }
|
|---|
| 152 | }
|
|---|
