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
51static double *scales;
52static double **lu;
53static 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
67int 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
142void 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