1 | /* $Id$ $Revision$ */ |
2 | /* vim:set shiftwidth=4 ts=8: */ |
3 | |
4 | /************************************************************************* |
5 | * Copyright (c) 2011 AT&T Intellectual Property |
6 | * All rights reserved. This program and the accompanying materials |
7 | * are made available under the terms of the Eclipse Public License v1.0 |
8 | * which accompanies this distribution, and is available at |
9 | * http://www.eclipse.org/legal/epl-v10.html |
10 | * |
11 | * Contributors: See CVS logs. Details at http://www.graphviz.org/ |
12 | *************************************************************************/ |
13 | |
14 | /* |
15 | * This code was (mostly) written by Ken Turkowski, who said: |
16 | * |
17 | * Oh, that. I wrote it in college the first time. It's open source - I think I |
18 | * posted it after seeing so many people solve equations by inverting matrices |
19 | * by computing minors naïvely. |
20 | * -Ken |
21 | * |
22 | * The views represented here are mine and are not necessarily shared by |
23 | * my employer. |
24 | Ken Turkowski turk@apple.com |
25 | Immersive Media Technologist http://www.worldserver.com/turk/ |
26 | Apple Computer, Inc. |
27 | 1 Infinite Loop, MS 302-3VR |
28 | Cupertino, CA 95014 |
29 | */ |
30 | |
31 | |
32 | |
33 | /* This module solves linear equations in several variables (Ax = b) using |
34 | * LU decomposition with partial pivoting and row equilibration. Although |
35 | * slightly more work than Gaussian elimination, it is faster for solving |
36 | * several equations using the same coefficient matrix. It is |
37 | * particularly useful for matrix inversion, by sequentially solving the |
38 | * equations with the columns of the unit matrix. |
39 | * |
40 | * lu_decompose() decomposes the coefficient matrix into the LU matrix, |
41 | * and lu_solve() solves the series of matrix equations using the |
42 | * previous LU decomposition. |
43 | * |
44 | * Ken Turkowski (apple!turk) |
45 | * written 3/2/79, revised and enhanced 8/9/83. |
46 | */ |
47 | |
48 | #include <math.h> |
49 | #include <neato.h> |
50 | |
51 | static double *scales; |
52 | static double **lu; |
53 | static int *ps; |
54 | |
55 | /* lu_decompose() decomposes the coefficient matrix A into upper and lower |
56 | * triangular matrices, the composite being the LU matrix. |
57 | * |
58 | * The arguments are: |
59 | * |
60 | * a - the (n x n) coefficient matrix |
61 | * n - the order of the matrix |
62 | * |
63 | * 1 is returned if the decomposition was successful, |
64 | * and 0 is returned if the coefficient matrix is singular. |
65 | */ |
66 | |
67 | int lu_decompose(double **a, int n) |
68 | { |
69 | register int i, j, k; |
70 | int pivotindex = 0; |
71 | double pivot, biggest, mult, tempf; |
72 | |
73 | if (lu) |
74 | free_array(lu); |
75 | lu = new_array(n, n, 0.0); |
76 | if (ps) |
77 | free(ps); |
78 | ps = N_NEW(n, int); |
79 | if (scales) |
80 | free(scales); |
81 | scales = N_NEW(n, double); |
82 | |
83 | for (i = 0; i < n; i++) { /* For each row */ |
84 | /* Find the largest element in each row for row equilibration */ |
85 | biggest = 0.0; |
86 | for (j = 0; j < n; j++) |
87 | if (biggest < (tempf = fabs(lu[i][j] = a[i][j]))) |
88 | biggest = tempf; |
89 | if (biggest != 0.0) |
90 | scales[i] = 1.0 / biggest; |
91 | else { |
92 | scales[i] = 0.0; |
93 | return (0); /* Zero row: singular matrix */ |
94 | } |
95 | ps[i] = i; /* Initialize pivot sequence */ |
96 | } |
97 | |
98 | for (k = 0; k < n - 1; k++) { /* For each column */ |
99 | /* Find the largest element in each column to pivot around */ |
100 | biggest = 0.0; |
101 | for (i = k; i < n; i++) { |
102 | if (biggest < (tempf = fabs(lu[ps[i]][k]) * scales[ps[i]])) { |
103 | biggest = tempf; |
104 | pivotindex = i; |
105 | } |
106 | } |
107 | if (biggest == 0.0) |
108 | return (0); /* Zero column: singular matrix */ |
109 | if (pivotindex != k) { /* Update pivot sequence */ |
110 | j = ps[k]; |
111 | ps[k] = ps[pivotindex]; |
112 | ps[pivotindex] = j; |
113 | } |
114 | |
115 | /* Pivot, eliminating an extra variable each time */ |
116 | pivot = lu[ps[k]][k]; |
117 | for (i = k + 1; i < n; i++) { |
118 | lu[ps[i]][k] = mult = lu[ps[i]][k] / pivot; |
119 | if (mult != 0.0) { |
120 | for (j = k + 1; j < n; j++) |
121 | lu[ps[i]][j] -= mult * lu[ps[k]][j]; |
122 | } |
123 | } |
124 | } |
125 | |
126 | if (lu[ps[n - 1]][n - 1] == 0.0) |
127 | return (0); /* Singular matrix */ |
128 | return (1); |
129 | } |
130 | |
131 | /* lu_solve() solves the linear equation (Ax = b) after the matrix A has |
132 | * been decomposed with lu_decompose() into the lower and upper triangular |
133 | * matrices L and U. |
134 | * |
135 | * The arguments are: |
136 | * |
137 | * x - the solution vector |
138 | * b - the constant vector |
139 | * n - the order of the equation |
140 | */ |
141 | |
142 | void lu_solve(double *x, double *b, int n) |
143 | { |
144 | register int i, j; |
145 | double dot; |
146 | |
147 | /* Vector reduction using U triangular matrix */ |
148 | for (i = 0; i < n; i++) { |
149 | dot = 0.0; |
150 | for (j = 0; j < i; j++) |
151 | dot += lu[ps[i]][j] * x[j]; |
152 | x[i] = b[ps[i]] - dot; |
153 | } |
154 | |
155 | /* Back substitution, in L triangular matrix */ |
156 | for (i = n - 1; i >= 0; i--) { |
157 | dot = 0.0; |
158 | for (j = i + 1; j < n; j++) |
159 | dot += lu[ps[i]][j] * x[j]; |
160 | x[i] = (x[i] - dot) / lu[ps[i]][i]; |
161 | } |
162 | } |
163 | |