/* * This code was (mostly) written by Ken Turkowski, who said: * * Oh, that. I wrote it in college the first time. It's open source - I think I * posted it after seeing so many people solve equations by inverting matrices * by computing minors naïvely. * -Ken * * The views represented here are mine and are not necessarily shared by * my employer. Ken Turkowski turk@apple.com Immersive Media Technologist http://www.worldserver.com/turk/ Apple Computer, Inc. 1 Infinite Loop, MS 302-3VR Cupertino, CA 95014 */ /* This module solves linear equations in several variables (Ax = b) using * LU decomposition with partial pivoting and row equilibration. Although * slightly more work than Gaussian elimination, it is faster for solving * several equations using the same coefficient matrix. It is * particularly useful for matrix inversion, by sequentially solving the * equations with the columns of the unit matrix. * * lu_decompose() decomposes the coefficient matrix into the LU matrix, * and lu_solve() solves the series of matrix equations using the * previous LU decomposition. * * Ken Turkowski (apple!turk) * written 3/2/79, revised and enhanced 8/9/83. */ #include <stdlib.h> #include <neato.h> static double *scales; static double **lu; static int *ps; /* lu_decompose() decomposes the coefficient matrix A into upper and lower * triangular matrices, the composite being the LU matrix. * * The arguments are: * * a - the (n x n) coefficient matrix * n - the order of the matrix * * 1 is returned if the decomposition was successful, * and 0 is returned if the coefficient matrix is singular. */ int lu_decompose(double **a, int n) { register int i, j, k; int pivotindex = 0; double pivot, biggest, mult, tempf; double fabs(); if (lu) free_array(lu); lu = new_array(n,n,0.0); if (ps) free(ps); ps = N_NEW(n,int); if (scales) free(scales); scales = N_NEW(n,double); for (i = 0; i < n; i++) { /* For each row */ /* Find the largest element in each row for row equilibration */ biggest = 0.0; for (j = 0; j < n; j++) if (biggest < (tempf = fabs(lu[i][j] = a[i][j]))) biggest = tempf; if (biggest != 0.0) scales[i] = 1.0 / biggest; else { scales[i] = 0.0; return(0); /* Zero row: singular matrix */ } ps[i] = i; /* Initialize pivot sequence */ } for (k = 0; k < n-1; k++) { /* For each column */ /* Find the largest element in each column to pivot around */ biggest = 0.0; for (i = k; i < n; i++) { if (biggest < (tempf = fabs(lu[ps[i]][k]) * scales[ps[i]])) { biggest = tempf; pivotindex = i; } } if (biggest == 0.0) return(0); /* Zero column: singular matrix */ if (pivotindex != k) { /* Update pivot sequence */ j = ps[k]; ps[k] = ps[pivotindex]; ps[pivotindex] = j; } /* Pivot, eliminating an extra variable each time */ pivot = lu[ps[k]][k]; for (i = k+1; i < n; i++) { lu[ps[i]][k] = mult = lu[ps[i]][k] / pivot; if (mult != 0.0) { for (j = k+1; j < n; j++) lu[ps[i]][j] -= mult * lu[ps[k]][j]; } } } if (lu[ps[n-1]][n-1] == 0.0) return(0); /* Singular matrix */ return(1); } /* lu_solve() solves the linear equation (Ax = b) after the matrix A has * been decomposed with lu_decompose() into the lower and upper triangular * matrices L and U. * * The arguments are: * * x - the solution vector * b - the constant vector * n - the order of the equation */ void lu_solve(double *x, double *b, int n) { register int i, j; double dot; /* Vector reduction using U triangular matrix */ for (i = 0; i < n; i++) { dot = 0.0; for (j = 0; j < i; j++) dot += lu[ps[i]][j] * x[j]; x[i] = b[ps[i]] - dot; } /* Back substitution, in L triangular matrix */ for (i = n-1; i >= 0; i--) { dot = 0.0; for (j = i+1; j < n; j++) dot += lu[ps[i]][j] * x[j]; x[i] = (x[i] - dot) / lu[ps[i]][i]; } }