1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
|
/*************************************************************************
* Copyright (c) 2011 AT&T Intellectual Property
* All rights reserved. This program and the accompanying materials
* are made available under the terms of the Eclipse Public License v1.0
* which accompanies this distribution, and is available at
* https://www.eclipse.org/legal/epl-v10.html
*
* Contributors: Details at https://graphviz.org
*************************************************************************/
/*
* 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 <math.h>
#include <neatogen/neato.h>
#include <util/alloc.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)
{
int i, j, k;
int pivotindex = 0;
double pivot, biggest, mult, tempf;
if (lu)
free_array(lu);
lu = new_array(n, n, 0.0);
free(ps);
ps = gv_calloc(n, sizeof(int));
free(scales);
scales = gv_calloc(n, sizeof(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++)
biggest = fmax(biggest, fabs(lu[i][j] = a[i][j]));
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;
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)
{
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];
}
}
|
