131 lines
3.4 KiB
C
131 lines
3.4 KiB
C
/*
|
|
* Copyright (c) 1986, 1987, 1988, 1989 by Sun Microsystems, Inc.
|
|
* Permission to use, copy, modify, and distribute this software for any
|
|
* purpose and without fee is hereby granted, provided that the above
|
|
* copyright notice appear in all copies and that both that copyright
|
|
* notice and this permission notice are retained, and that the name
|
|
* of Sun Microsystems, Inc., not be used in advertising or publicity
|
|
* pertaining to this software without specific, written prior permission.
|
|
* Sun Microsystems, Inc., makes no representations about the suitability
|
|
* of this software or the interface defined in this software for any
|
|
* purpose. It is provided "as is" without express or implied warranty.
|
|
*/
|
|
#ifndef lint
|
|
static char sccsid[] = "@(#)matinv.c 1.1 94/10/31 Copyr 1986 Sun Micro";
|
|
#endif
|
|
|
|
/*
|
|
* Copyright (c) 1986 by Sun Microsystems, Inc.
|
|
*/
|
|
|
|
/*
|
|
*
|
|
* _core_matinv - Matrix inversion with solution of linear equations
|
|
*
|
|
* Operation:
|
|
* Performs a matrix inversion on up to a 16 by 16 matrix solving
|
|
* the associated set of linear equations and returning the value
|
|
* of the determinant.
|
|
* Usage:
|
|
* float a(nt,nt) ! matrix to invert nt by nt square
|
|
* int nt ! size of matrix (2..16)
|
|
* double b(nt,m) ! m sets of nt input values to be solved
|
|
* ! returned solutions to m equations.
|
|
* int m ! number of equations to solve. (0..4)
|
|
* double determ ! returned value of determinant
|
|
*
|
|
* _core_matinv( a,nt,b,m,&determ);
|
|
*/
|
|
|
|
#include <math.h>
|
|
#define elem(a,i,j) *(a+(a/**/x * (i) + (j))
|
|
|
|
_core_matinv(a, n, b, m, determ)
|
|
float a[4][4];
|
|
int n, m;
|
|
double b[4][2], *determ;
|
|
|
|
{
|
|
short index[16][2], ipivot[16];
|
|
float pivot[16];
|
|
short row, colum;
|
|
float max;
|
|
short i, j, k, l;
|
|
|
|
*determ = 1.0; /* Initialization */
|
|
for (j = 0; j < n; j++)
|
|
ipivot[j] = 0;
|
|
|
|
for (i = 0; i < n; i++) { /* do matrix inversion */
|
|
max = 0.0;
|
|
for (j = 0; j < n; j++) { /* search for pivot element */
|
|
if (ipivot[j] == 1)
|
|
continue;
|
|
for (k = 0; k < n; k++) {
|
|
if (ipivot[k] == 1)
|
|
continue;
|
|
if (ipivot[k] > 1)
|
|
return (0);
|
|
if (fabs(max) < fabs(a[j][k])) {
|
|
row = j;
|
|
colum = k;
|
|
max = a[j][k];
|
|
}
|
|
}
|
|
}
|
|
if (max == 0.0) {
|
|
*determ = 0.0;
|
|
return (0);
|
|
}
|
|
ipivot[colum] += 1;
|
|
if (row != colum) { /* interchange rows to put */
|
|
*determ = -*determ; /* pivot element on diagonal */
|
|
for (l = 0; l < n; l++) {
|
|
max = a[row][l];
|
|
a[row][l] = a[colum][l];
|
|
a[colum][l] = max;
|
|
}
|
|
if (m > 0)
|
|
for (l = 0; l < m; l++) {
|
|
max = b[row][l];
|
|
b[row][l] = b[colum][l];
|
|
b[colum][l] = max;
|
|
}
|
|
}
|
|
index[i][0] = row;
|
|
index[i][1] = colum;
|
|
pivot[i] = a[colum][colum];
|
|
*determ *= pivot[i];
|
|
a[colum][colum] = 1.0; /* divide pivot row by pivot element */
|
|
for (l = 0; l < n; l++)
|
|
a[colum][l] /= pivot[i];
|
|
if (m > 0)
|
|
for (l = 0; l < m; l++)
|
|
b[colum][l] /= pivot[i];
|
|
for (j = 0; j < n; j++)
|
|
if (j != colum) {
|
|
max = a[j][colum];
|
|
a[j][colum] = 0.0;
|
|
for (l = 0; l < n; l++)
|
|
a[j][l] -= a[colum][l] * max;
|
|
if (m > 0)
|
|
for (l = 0; l < n; l++)
|
|
b[j][l] -= b[colum][l] * max;
|
|
}
|
|
}
|
|
|
|
for (i = 0; i < n; i++) { /* interchange columns */
|
|
l = n - 1 - i;
|
|
if (index[l][0] != index[l][1]) {
|
|
row = index[l][0];
|
|
colum = index[l][1];
|
|
for (k = 0; k < n; k++) {
|
|
max = a[k][row];
|
|
a[k][row] = a[k][colum];
|
|
a[k][colum] = max;
|
|
}
|
|
}
|
|
}
|
|
return (0);
|
|
}
|