00001 /* $Id: lu.c,v 1.2 2005/02/23 18:58:12 ellson Exp $ $Revision: 1.2 $ */ 00002 /* vim:set shiftwidth=4 ts=8: */ 00003 00004 /********************************************************** 00005 * This software is part of the graphviz package * 00006 * http://www.graphviz.org/ * 00007 * * 00008 * Copyright (c) 1994-2004 AT&T Corp. * 00009 * and is licensed under the * 00010 * Common Public License, Version 1.0 * 00011 * by AT&T Corp. * 00012 * * 00013 * Information and Software Systems Research * 00014 * AT&T Research, Florham Park NJ * 00015 **********************************************************/ 00016 00017 /* 00018 * This code was (mostly) written by Ken Turkowski, who said: 00019 * 00020 * Oh, that. I wrote it in college the first time. It's open source - I think I 00021 * posted it after seeing so many people solve equations by inverting matrices 00022 * by computing minors naïvely. 00023 * -Ken 00024 * 00025 * The views represented here are mine and are not necessarily shared by 00026 * my employer. 00027 Ken Turkowski turk@apple.com 00028 Immersive Media Technologist http://www.worldserver.com/turk/ 00029 Apple Computer, Inc. 00030 1 Infinite Loop, MS 302-3VR 00031 Cupertino, CA 95014 00032 */ 00033 00034 00035 00036 /* This module solves linear equations in several variables (Ax = b) using 00037 * LU decomposition with partial pivoting and row equilibration. Although 00038 * slightly more work than Gaussian elimination, it is faster for solving 00039 * several equations using the same coefficient matrix. It is 00040 * particularly useful for matrix inversion, by sequentially solving the 00041 * equations with the columns of the unit matrix. 00042 * 00043 * lu_decompose() decomposes the coefficient matrix into the LU matrix, 00044 * and lu_solve() solves the series of matrix equations using the 00045 * previous LU decomposition. 00046 * 00047 * Ken Turkowski (apple!turk) 00048 * written 3/2/79, revised and enhanced 8/9/83. 00049 */ 00050 00051 #include <math.h> 00052 #include <neato.h> 00053 00054 static double *scales; 00055 static double **lu; 00056 static int *ps; 00057 00058 /* lu_decompose() decomposes the coefficient matrix A into upper and lower 00059 * triangular matrices, the composite being the LU matrix. 00060 * 00061 * The arguments are: 00062 * 00063 * a - the (n x n) coefficient matrix 00064 * n - the order of the matrix 00065 * 00066 * 1 is returned if the decomposition was successful, 00067 * and 0 is returned if the coefficient matrix is singular. 00068 */ 00069 00070 int lu_decompose(double **a, int n) 00071 { 00072 register int i, j, k; 00073 int pivotindex = 0; 00074 double pivot, biggest, mult, tempf; 00075 00076 if (lu) 00077 free_array(lu); 00078 lu = new_array(n, n, 0.0); 00079 if (ps) 00080 free(ps); 00081 ps = N_NEW(n, int); 00082 if (scales) 00083 free(scales); 00084 scales = N_NEW(n, double); 00085 00086 for (i = 0; i < n; i++) { /* For each row */ 00087 /* Find the largest element in each row for row equilibration */ 00088 biggest = 0.0; 00089 for (j = 0; j < n; j++) 00090 if (biggest < (tempf = fabs(lu[i][j] = a[i][j]))) 00091 biggest = tempf; 00092 if (biggest != 0.0) 00093 scales[i] = 1.0 / biggest; 00094 else { 00095 scales[i] = 0.0; 00096 return (0); /* Zero row: singular matrix */ 00097 } 00098 ps[i] = i; /* Initialize pivot sequence */ 00099 } 00100 00101 for (k = 0; k < n - 1; k++) { /* For each column */ 00102 /* Find the largest element in each column to pivot around */ 00103 biggest = 0.0; 00104 for (i = k; i < n; i++) { 00105 if (biggest < (tempf = fabs(lu[ps[i]][k]) * scales[ps[i]])) { 00106 biggest = tempf; 00107 pivotindex = i; 00108 } 00109 } 00110 if (biggest == 0.0) 00111 return (0); /* Zero column: singular matrix */ 00112 if (pivotindex != k) { /* Update pivot sequence */ 00113 j = ps[k]; 00114 ps[k] = ps[pivotindex]; 00115 ps[pivotindex] = j; 00116 } 00117 00118 /* Pivot, eliminating an extra variable each time */ 00119 pivot = lu[ps[k]][k]; 00120 for (i = k + 1; i < n; i++) { 00121 lu[ps[i]][k] = mult = lu[ps[i]][k] / pivot; 00122 if (mult != 0.0) { 00123 for (j = k + 1; j < n; j++) 00124 lu[ps[i]][j] -= mult * lu[ps[k]][j]; 00125 } 00126 } 00127 } 00128 00129 if (lu[ps[n - 1]][n - 1] == 0.0) 00130 return (0); /* Singular matrix */ 00131 return (1); 00132 } 00133 00134 /* lu_solve() solves the linear equation (Ax = b) after the matrix A has 00135 * been decomposed with lu_decompose() into the lower and upper triangular 00136 * matrices L and U. 00137 * 00138 * The arguments are: 00139 * 00140 * x - the solution vector 00141 * b - the constant vector 00142 * n - the order of the equation 00143 */ 00144 00145 void lu_solve(double *x, double *b, int n) 00146 { 00147 register int i, j; 00148 double dot; 00149 00150 /* Vector reduction using U triangular matrix */ 00151 for (i = 0; i < n; i++) { 00152 dot = 0.0; 00153 for (j = 0; j < i; j++) 00154 dot += lu[ps[i]][j] * x[j]; 00155 x[i] = b[ps[i]] - dot; 00156 } 00157 00158 /* Back substitution, in L triangular matrix */ 00159 for (i = n - 1; i >= 0; i--) { 00160 dot = 0.0; 00161 for (j = i + 1; j < n; j++) 00162 dot += lu[ps[i]][j] * x[j]; 00163 x[i] = (x[i] - dot) / lu[ps[i]][i]; 00164 } 00165 }