1 | #include <math.h> |
---|
2 | #include "lupack.h" |
---|
3 | |
---|
4 | #define ORDER 1000 /* maksimalna mo"zna velikost linearnega sistema */ |
---|
5 | #define TINY 1e-5 |
---|
6 | |
---|
7 | /**************************************************************************/ |
---|
8 | /*+ |
---|
9 | This function replaces the matrix m by the LU decomposition of a rowwise |
---|
10 | permutation of itself. indx is an output vector which records the |
---|
11 | row permutation effected by the partial pivoting. d is output as 1 or -1 |
---|
12 | depending on whether the number of row interchanges was even or odd. |
---|
13 | Vrne -1 ob napaki; |
---|
14 | -*/ |
---|
15 | /**************************************************************************/ |
---|
16 | |
---|
17 | int ludcmp( |
---|
18 | float *m, /* (I/O) input matrix and its decomp.*/ |
---|
19 | int n, /* velikost sistema */ |
---|
20 | int indx[], /* (O) permutation vector */ |
---|
21 | float *d) /* (O) 1 or -1 */ |
---|
22 | { |
---|
23 | int i, imax=0, j, k; |
---|
24 | float big, dum, sum, temp; |
---|
25 | float vv[ORDER]; |
---|
26 | |
---|
27 | *d = 1.0; |
---|
28 | for (i=0; i<n; i++) { |
---|
29 | big = 0.0; |
---|
30 | for (j=0; j<n; j++) |
---|
31 | if ((temp=fabs(m[i*n+j]))>big) big = temp; |
---|
32 | if (big==0.0) return -1; |
---|
33 | vv[i] = 1.0/big; |
---|
34 | } |
---|
35 | for (j=0; j<n; j++) { |
---|
36 | for (i=0; i<j; i++) { |
---|
37 | sum = m[i*n+j]; |
---|
38 | for (k=0; k<i; k++) sum -= m[i*n+k]*m[k*n+j]; |
---|
39 | m[i*n+j] = sum; |
---|
40 | } |
---|
41 | big = 0.0; |
---|
42 | for (i=j; i<n; i++) { |
---|
43 | sum = m[i*n+j]; |
---|
44 | for (k=0; k<j; k++) sum -= m[i*n+k]*m[k*n+j]; |
---|
45 | m[i*n+j] = sum; |
---|
46 | if ((dum=vv[i]*fabs(sum))>=big) { |
---|
47 | big = dum; |
---|
48 | imax = i; |
---|
49 | } |
---|
50 | } |
---|
51 | if (j!=imax) { |
---|
52 | for (k=0; k<n; k++) { |
---|
53 | dum = m[imax*n+k]; |
---|
54 | m[imax*n+k] = m[j*n+k]; |
---|
55 | m[j*n+k] = dum; |
---|
56 | } |
---|
57 | *d = -(*d); |
---|
58 | vv[imax] = vv[j]; |
---|
59 | } |
---|
60 | indx[j] = imax; |
---|
61 | if (m[j*n+j]==0.0) m[j*n+j] = TINY; |
---|
62 | if (j!=n-1) { |
---|
63 | dum = 1.0/(m[j*n+j]); |
---|
64 | for (i=j+1; i<n; i++) m[i*n+j] *= dum; |
---|
65 | } |
---|
66 | } |
---|
67 | return 0; |
---|
68 | |
---|
69 | /* END GL_LUDCMP FUNCTION */ |
---|
70 | } |
---|
71 | |
---|
72 | |
---|
73 | /**************************************************************************/ |
---|
74 | /*+ |
---|
75 | This function solves the set of n (ORDER) linear equations MX=B. The |
---|
76 | matrix m is not as the matrix M but rather as its LU decomposition, |
---|
77 | returned by ludcmp. indx is the permutation vector also returned by |
---|
78 | ludcmp. b is the right-hand side vector B and returns the solution |
---|
79 | vector X. |
---|
80 | -*/ |
---|
81 | /**************************************************************************/ |
---|
82 | int lubksb( |
---|
83 | float *m, /* (I) LU decomposition coef. matrix */ |
---|
84 | int n, /* velikost sistema */ |
---|
85 | int indx[], /* (I) permutation vector */ |
---|
86 | float b[]) /* (I) RHS vector and solution vector */ |
---|
87 | { |
---|
88 | int i, id=0, ii = 0, ip, j; |
---|
89 | float sum; |
---|
90 | |
---|
91 | for (i=0; i<n; i++) { |
---|
92 | ip = indx[i]; |
---|
93 | sum = b[ip]; |
---|
94 | b[ip] = b[i]; |
---|
95 | if (id) |
---|
96 | for (j=ii; j<=i-1; j++) sum -= m[i*n+j]*b[j]; |
---|
97 | else |
---|
98 | if (sum) { |
---|
99 | ii = i; |
---|
100 | id = 1; |
---|
101 | } |
---|
102 | b[i] = sum; |
---|
103 | } |
---|
104 | for (i=n-1; i>=0; i--) { |
---|
105 | sum = b[i]; |
---|
106 | for (j=i+1; j<n; j++) sum -= m[i*n+j]*b[j]; |
---|
107 | b[i] = sum/m[i*n+i]; |
---|
108 | } |
---|
109 | return 0; |
---|
110 | |
---|
111 | /* END GL_LUBKSB FUNCTION */ |
---|
112 | } |
---|
113 | |
---|
114 | |
---|
115 | |
---|
116 | |
---|
117 | |
---|
118 | |
---|
119 | |
---|
120 | |
---|