1// This file is part of SmallBASIC
2//
3// Math RTL
4//
5// This program is distributed under the terms of the GPL v2.0 or later
6// Download the GNU Public License (GPL) from www.gnu.org
7//
8// Original headers from matrx042.zip follow:
9
10/*-----------------------------------------------------------------------------
11 * desc: matrix mathematics
12 * by: ko shu pui, patrick
13 * date: 24 nov 91 v0.1b
14 * ref:
15 * [1] Mary L.Boas, "Mathematical Methods in the Physical Sciene,"
16 * John Wiley & Sons, 2nd Ed., 1983. Chap 3.
17 *
18 * [2] Kendall E.Atkinson, "An Introduction to Numberical Analysis,"
19 * John Wiley & Sons, 1978.
20 *
21 * [3] Alfred V.Aho, John E.Hopcroft, Jeffrey D.Ullman, "The Design
22 * and Analysis of Computer Algorithms," 1974.
23 *
24 *----------------------------------------------------------------------------*/
25
26#include "common/sys.h"
27#include "common/blib_math.h"
28
29// creates a matrix of the given size
30var_num_t **mat_create(int row, int col) {
31 var_num_t **m = (var_num_t **)malloc(sizeof(var_num_t *) * row);
32 int i;
33 for (i = 0; i < row; i++) {
34 m[i] = (var_num_t *)malloc(sizeof(var_num_t) * col);
35 }
36
37 return m;
38}
39
40// free an allocated matrix
41void mat_free(var_num_t **m, int n) {
42 int i;
43 for (i = 0; i < n; i++) {
44 free(m[i]);
45 }
46 free(m);
47}
48
49// fill the matrix with zero values
50void mat_fill(var_num_t **A, int row, int col) {
51 int i, j;
52
53 for (i = 0; i < row; i++) {
54 for (j = 0; j < col; j++) {
55 A[i][j] = 0.0;
56 }
57 }
58}
59
60/*
61 *-----------------------------------------------------------------------------
62 * funct: mat_lu
63 * desct: in-place LU decomposition with partial pivoting
64 * given: !! A = square matrix (n x n) !ATTENTION! see commen
65 * P = permutation vector (n x 1)
66 * retrn: number of permutation performed
67 * -1 means suspected singular matrix
68 * comen: A will be overwritten to be a LU-composite matrix
69 *
70 * note: the LU decomposed may NOT be equal to the LU of
71 * the orignal matrix a. But equal to the LU of the
72 * rows interchanged matrix.
73 *-----------------------------------------------------------------------------
74 */
75int mat_lu(var_num_t **A, var_num_t **P, int n) {
76 int i, j, k, maxi, tmp, p;
77 var_num_t c, c1;
78
79 for (p = 0, i = 0; i < n; i++) {
80 P[i][0] = i;
81 }
82
83 for (k = 0; k < n; k++) {
84 /*
85 * --- partial pivoting ---
86 */
87 for (i = k, maxi = k, c = 0.0; i < n; i++) {
88 c1 = fabsl(A[(int) P[i][0]][k]);
89 if (c1 > c) {
90 c = c1;
91 maxi = i;
92 }
93 }
94
95 /*
96 * row exchange, update permutation vector
97 */
98 if (k != maxi) {
99 p++;
100 tmp = P[k][0];
101 P[k][0] = P[maxi][0];
102 P[maxi][0] = tmp;
103 }
104
105 /*
106 * suspected singular matrix
107 */
108 if (A[(int) P[k][0]][k] == 0.0) {
109 return -1;
110 }
111
112 for (i = k + 1; i < n; i++) {
113 /*
114 * --- calculate m(i,j) ---
115 */
116 A[(int) P[i][0]][k] = A[(int) P[i][0]][k] / A[(int) P[k][0]][k];
117
118 /*
119 * --- elimination ---
120 */
121 for (j = k + 1; j < n; j++) {
122 A[(int) P[i][0]][j] -= A[(int) P[i][0]][k] * A[(int) P[k][0]][j];
123 }
124 }
125 }
126
127 return p;
128}
129
130/*
131 *-----------------------------------------------------------------------------
132 * funct: mat_backsubs1
133 * desct: back substitution
134 * given: A = square matrix A (LU composite)
135 * !! B = column matrix B (attention!, see comen)
136 * !! X = place to put the result of X
137 * P = Permutation vector (after calling mat_lu)
138 * xcol = column of x to put the result
139 * retrn: column matrix X (of AX = B)
140 * comen: B will be overwritten
141 *-----------------------------------------------------------------------------
142 */
143var_num_t **mat_backsubs1(var_num_t **A, var_num_t **B, var_num_t **X, var_num_t **P, int xcol, int n) {
144 int i, j, k;
145 var_num_t sum;
146
147 for (k = 0; k < n; k++) {
148 for (i = k + 1; i < n; i++) {
149 B[(int) P[i][0]][0] -= A[(int) P[i][0]][k] * B[(int) P[k][0]][0];
150 }
151 }
152
153 X[n - 1][xcol] = B[(int) P[n - 1][0]][0] / A[(int) P[n - 1][0]][n - 1];
154 for (k = n - 2; k >= 0; k--) {
155 sum = 0.0;
156 for (j = k + 1; j < n; j++) {
157 sum += A[(int) P[k][0]][j] * X[j][xcol];
158 }
159 X[k][xcol] = (B[(int) P[k][0]][0] - sum) / A[(int) P[k][0]][k];
160 }
161
162 return X;
163}
164
165/*
166 *-----------------------------------------------------------------------------
167 * funct: mat_inv
168 * desct: find inverse of a matrix
169 * given: a = square matrix a
170 * retrn: square matrix Inverse(A)
171 * NULL = fails, singular matrix, or malloc() fails
172 *-----------------------------------------------------------------------------
173 */
174void mat_inverse(var_num_t *a, const int n) {
175 var_num_t **A = mat_create(n, n);
176 var_num_t **B = mat_create(n, 1);
177 var_num_t **C = mat_create(n, n);
178 var_num_t **P = mat_create(n, 1);
179
180 int i, j;
181
182 // copy input matrix to working buffer
183 for (i = 0; i < n; i++) {
184 for (j = 0; j < n; j++) {
185 A[i][j] = a[i * n + j];
186 }
187 }
188
189 // LU-decomposition, also check for singular matrix
190 if (mat_lu(A, P, n) != -1) {
191 for (i = 0; i < n; i++) {
192 mat_fill(B, n, 1);
193 B[i][0] = 1.0;
194 mat_backsubs1(A, B, C, P, i, n);
195 }
196
197 // copy the result in C back to a
198 for (i = 0; i < n; i++) {
199 for (j = 0; j < n; j++) {
200 a[i * n + j] = C[i][j];
201 }
202 }
203 }
204
205 // release memory
206 mat_free(P, n);
207 mat_free(C, n);
208 mat_free(B, n);
209 mat_free(A, n);
210}
211