doxygen: intern/smoke tagged.
[blender.git] / intern / smoke / intern / tnt / jama_lu.h
1 /** \file smoke/intern/tnt/jama_lu.h
2  *  \ingroup smoke
3  */
4 #ifndef JAMA_LU_H
5 #define JAMA_LU_H
6
7 #include "tnt.h"
8 #include <algorithm>
9 //for min(), max() below
10
11 using namespace TNT;
12 using namespace std;
13
14 namespace JAMA
15 {
16
17    /** LU Decomposition.
18    <P>
19    For an m-by-n matrix A with m >= n, the LU decomposition is an m-by-n
20    unit lower triangular matrix L, an n-by-n upper triangular matrix U,
21    and a permutation vector piv of length m so that A(piv,:) = L*U.
22    If m < n, then L is m-by-m and U is m-by-n.
23    <P>
24    The LU decompostion with pivoting always exists, even if the matrix is
25    singular, so the constructor will never fail.  The primary use of the
26    LU decomposition is in the solution of square systems of simultaneous
27    linear equations.  This will fail if isNonsingular() returns false.
28    */
29 template <class Real>
30 class LU
31 {
32
33
34
35    /* Array for internal storage of decomposition.  */
36    Array2D<Real>  LU_;
37    int m, n, pivsign; 
38    Array1D<int> piv;
39
40
41    Array2D<Real> permute_copy(const Array2D<Real> &A, 
42                         const Array1D<int> &piv, int j0, int j1)
43         {
44                 int piv_length = piv.dim();
45
46                 Array2D<Real> X(piv_length, j1-j0+1);
47
48
49          for (int i = 0; i < piv_length; i++) 
50             for (int j = j0; j <= j1; j++) 
51                X[i][j-j0] = A[piv[i]][j];
52
53                 return X;
54         }
55
56    Array1D<Real> permute_copy(const Array1D<Real> &A, 
57                 const Array1D<int> &piv)
58         {
59                 int piv_length = piv.dim();
60                 if (piv_length != A.dim())
61                         return Array1D<Real>();
62
63                 Array1D<Real> x(piv_length);
64
65
66          for (int i = 0; i < piv_length; i++) 
67                x[i] = A[piv[i]];
68
69                 return x;
70         }
71
72
73         public :
74
75    /** LU Decomposition
76    @param  A   Rectangular matrix
77    @return     LU Decomposition object to access L, U and piv.
78    */
79
80     LU (const Array2D<Real> &A) : LU_(A.copy()), m(A.dim1()), n(A.dim2()), 
81                 piv(A.dim1())
82         
83         {
84
85    // Use a "left-looking", dot-product, Crout/Doolittle algorithm.
86
87
88       for (int i = 0; i < m; i++) {
89          piv[i] = i;
90       }
91       pivsign = 1;
92       Real *LUrowi = 0;;
93       Array1D<Real> LUcolj(m);
94
95       // Outer loop.
96
97       for (int j = 0; j < n; j++) {
98
99          // Make a copy of the j-th column to localize references.
100
101          for (int i = 0; i < m; i++) {
102             LUcolj[i] = LU_[i][j];
103          }
104
105          // Apply previous transformations.
106
107          for (int i = 0; i < m; i++) {
108             LUrowi = LU_[i];
109
110             // Most of the time is spent in the following dot product.
111
112             int kmax = min(i,j);
113             double s = 0.0;
114             for (int k = 0; k < kmax; k++) {
115                s += LUrowi[k]*LUcolj[k];
116             }
117
118             LUrowi[j] = LUcolj[i] -= s;
119          }
120    
121          // Find pivot and exchange if necessary.
122
123          int p = j;
124          for (int i = j+1; i < m; i++) {
125             if (abs(LUcolj[i]) > abs(LUcolj[p])) {
126                p = i;
127             }
128          }
129          if (p != j) {
130                     int k=0;
131             for (k = 0; k < n; k++) {
132                double t = LU_[p][k]; 
133                            LU_[p][k] = LU_[j][k]; 
134                            LU_[j][k] = t;
135             }
136             k = piv[p]; 
137                         piv[p] = piv[j]; 
138                         piv[j] = k;
139             pivsign = -pivsign;
140          }
141
142          // Compute multipliers.
143          
144          if ((j < m) && (LU_[j][j] != 0.0)) {
145             for (int i = j+1; i < m; i++) {
146                LU_[i][j] /= LU_[j][j];
147             }
148          }
149       }
150    }
151
152
153    /** Is the matrix nonsingular?
154    @return     1 (true)  if upper triangular factor U (and hence A) 
155                                 is nonsingular, 0 otherwise.
156    */
157
158    int isNonsingular () {
159       for (int j = 0; j < n; j++) {
160          if (LU_[j][j] == 0)
161             return 0;
162       }
163       return 1;
164    }
165
166    /** Return lower triangular factor
167    @return     L
168    */
169
170    Array2D<Real> getL () {
171       Array2D<Real> L_(m,n);
172       for (int i = 0; i < m; i++) {
173          for (int j = 0; j < n; j++) {
174             if (i > j) {
175                L_[i][j] = LU_[i][j];
176             } else if (i == j) {
177                L_[i][j] = 1.0;
178             } else {
179                L_[i][j] = 0.0;
180             }
181          }
182       }
183       return L_;
184    }
185
186    /** Return upper triangular factor
187    @return     U portion of LU factorization.
188    */
189
190    Array2D<Real> getU () {
191           Array2D<Real> U_(n,n);
192       for (int i = 0; i < n; i++) {
193          for (int j = 0; j < n; j++) {
194             if (i <= j) {
195                U_[i][j] = LU_[i][j];
196             } else {
197                U_[i][j] = 0.0;
198             }
199          }
200       }
201       return U_;
202    }
203
204    /** Return pivot permutation vector
205    @return     piv
206    */
207
208    Array1D<int> getPivot () {
209       return piv;
210    }
211
212
213    /** Compute determinant using LU factors.
214    @return     determinant of A, or 0 if A is not square.
215    */
216
217    Real det () {
218       if (m != n) {
219          return Real(0);
220       }
221       Real d = Real(pivsign);
222       for (int j = 0; j < n; j++) {
223          d *= LU_[j][j];
224       }
225       return d;
226    }
227
228    /** Solve A*X = B
229    @param  B   A Matrix with as many rows as A and any number of columns.
230    @return     X so that L*U*X = B(piv,:), if B is nonconformant, returns
231                                         0x0 (null) array.
232    */
233
234    Array2D<Real> solve (const Array2D<Real> &B) 
235    {
236
237           /* Dimensions: A is mxn, X is nxk, B is mxk */
238       
239       if (B.dim1() != m) {
240                 return Array2D<Real>(0,0);
241       }
242       if (!isNonsingular()) {
243         return Array2D<Real>(0,0);
244       }
245
246       // Copy right hand side with pivoting
247       int nx = B.dim2();
248
249
250           Array2D<Real> X = permute_copy(B, piv, 0, nx-1);
251
252       // Solve L*Y = B(piv,:)
253       for (int k = 0; k < n; k++) {
254          for (int i = k+1; i < n; i++) {
255             for (int j = 0; j < nx; j++) {
256                X[i][j] -= X[k][j]*LU_[i][k];
257             }
258          }
259       }
260       // Solve U*X = Y;
261       for (int k = n-1; k >= 0; k--) {
262          for (int j = 0; j < nx; j++) {
263             X[k][j] /= LU_[k][k];
264          }
265          for (int i = 0; i < k; i++) {
266             for (int j = 0; j < nx; j++) {
267                X[i][j] -= X[k][j]*LU_[i][k];
268             }
269          }
270       }
271       return X;
272    }
273
274
275    /** Solve A*x = b, where x and b are vectors of length equal 
276                 to the number of rows in A.
277
278    @param  b   a vector (Array1D> of length equal to the first dimension
279                                                 of A.
280    @return x a vector (Array1D> so that L*U*x = b(piv), if B is nonconformant,
281                                         returns 0x0 (null) array.
282    */
283
284    Array1D<Real> solve (const Array1D<Real> &b) 
285    {
286
287           /* Dimensions: A is mxn, X is nxk, B is mxk */
288       
289       if (b.dim1() != m) {
290                 return Array1D<Real>();
291       }
292       if (!isNonsingular()) {
293         return Array1D<Real>();
294       }
295
296
297           Array1D<Real> x = permute_copy(b, piv);
298
299       // Solve L*Y = B(piv)
300       for (int k = 0; k < n; k++) {
301          for (int i = k+1; i < n; i++) {
302                x[i] -= x[k]*LU_[i][k];
303             }
304          }
305       
306           // Solve U*X = Y;
307       for (int k = n-1; k >= 0; k--) {
308             x[k] /= LU_[k][k];
309                 for (int i = 0; i < k; i++) 
310                 x[i] -= x[k]*LU_[i][k];
311       }
312      
313
314       return x;
315    }
316
317 }; /* class LU */
318
319 } /* namespace JAMA */
320
321 #endif
322 /* JAMA_LU_H */