b86027aa386a13653785bb3d235694b4cfd1e78c
[blender.git] / intern / iksolver / intern / TNT / lu.h
1 /**
2  * $Id$
3  * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
4  *
5  * This program is free software; you can redistribute it and/or
6  * modify it under the terms of the GNU General Public License
7  * as published by the Free Software Foundation; either version 2
8  * of the License, or (at your option) any later version. The Blender
9  * Foundation also sells licenses for use in proprietary software under
10  * the Blender License.  See http://www.blender.org/BL/ for information
11  * about this.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with this program; if not, write to the Free Software Foundation,
20  * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
21  *
22  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
23  * All rights reserved.
24  *
25  * The Original Code is: all of this file.
26  *
27  * Contributor(s): none yet.
28  *
29  * ***** END GPL/BL DUAL LICENSE BLOCK *****
30  */
31
32 /*
33
34 *
35 * Template Numerical Toolkit (TNT): Linear Algebra Module
36 *
37 * Mathematical and Computational Sciences Division
38 * National Institute of Technology,
39 * Gaithersburg, MD USA
40 *
41 *
42 * This software was developed at the National Institute of Standards and
43 * Technology (NIST) by employees of the Federal Government in the course
44 * of their official duties. Pursuant to title 17 Section 105 of the
45 * United States Code, this software is not subject to copyright protection
46 * and is in the public domain.  The Template Numerical Toolkit (TNT) is
47 * an experimental system.  NIST assumes no responsibility whatsoever for
48 * its use by other parties, and makes no guarantees, expressed or implied,
49 * about its quality, reliability, or any other characteristic.
50 *
51 * BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE
52 * see http://math.nist.gov/tnt for latest updates.
53 *
54 */
55
56
57
58 #ifndef LU_H
59 #define LU_H
60
61 // Solve system of linear equations Ax = b.
62 //
63 //  Typical usage:
64 //
65 //    Matrix(double) A;
66 //    Vector(Subscript) ipiv;
67 //    Vector(double) b;
68 //
69 //    1)  LU_Factor(A,ipiv);
70 //    2)  LU_Solve(A,ipiv,b);
71 //
72 //   Now b has the solution x.  Note that both A and b
73 //   are overwritten.  If these values need to be preserved, 
74 //   one can make temporary copies, as in 
75 //
76 //    O)  Matrix(double) T = A;
77 //    1)  LU_Factor(T,ipiv);
78 //    1a) Vector(double) x=b;
79 //    2)  LU_Solve(T,ipiv,x);
80 //
81 //   See details below.
82 //
83
84
85 // for fabs() 
86 //
87 #include <cmath>
88
89 // right-looking LU factorization algorithm (unblocked)
90 //
91 //   Factors matrix A into lower and upper  triangular matrices 
92 //   (L and U respectively) in solving the linear equation Ax=b.  
93 //
94 //
95 // Args:
96 //
97 // A        (input/output) Matrix(1:n, 1:n)  In input, matrix to be
98 //                  factored.  On output, overwritten with lower and 
99 //                  upper triangular factors.
100 //
101 // indx     (output) Vector(1:n)    Pivot vector. Describes how
102 //                  the rows of A were reordered to increase
103 //                  numerical stability.
104 //
105 // Return value:
106 //
107 // int      (0 if successful, 1 otherwise)
108 //
109 //
110
111
112 namespace TNT
113 {
114
115 template <class MaTRiX, class VecToRSubscript>
116 int LU_factor( MaTRiX &A, VecToRSubscript &indx)
117 {
118     assert(A.lbound() == 1);                // currently for 1-offset
119     assert(indx.lbound() == 1);             // vectors and matrices
120
121     Subscript M = A.num_rows();
122     Subscript N = A.num_cols();
123
124     if (M == 0 || N==0) return 0;
125     if (indx.dim() != M)
126         indx.newsize(M);
127
128     Subscript i=0,j=0,k=0;
129     Subscript jp=0;
130
131     typename MaTRiX::element_type t;
132
133     Subscript minMN =  (M < N ? M : N) ;        // min(M,N);
134
135     for (j=1; j<= minMN; j++)
136     {
137
138         // find pivot in column j and  test for singularity.
139
140         jp = j;
141         t = fabs(A(j,j));
142         for (i=j+1; i<=M; i++)
143             if ( fabs(A(i,j)) > t)
144             {
145                 jp = i;
146                 t = fabs(A(i,j));
147             }
148
149         indx(j) = jp;
150
151         // jp now has the index of maximum element 
152         // of column j, below the diagonal
153
154         if ( A(jp,j) == 0 )                 
155             return 1;       // factorization failed because of zero pivot
156
157
158         if (jp != j)            // swap rows j and jp
159             for (k=1; k<=N; k++)
160             {
161                 t = A(j,k);
162                 A(j,k) = A(jp,k);
163                 A(jp,k) =t;
164             }
165
166         if (j<M)                // compute elements j+1:M of jth column
167         {
168             // note A(j,j), was A(jp,p) previously which was
169             // guarranteed not to be zero (Label #1)
170             //
171             typename MaTRiX::element_type recp =  1.0 / A(j,j);
172
173             for (k=j+1; k<=M; k++)
174                 A(k,j) *= recp;
175         }
176
177
178         if (j < minMN)
179         {
180             // rank-1 update to trailing submatrix:   E = E - x*y;
181             //
182             // E is the region A(j+1:M, j+1:N)
183             // x is the column vector A(j+1:M,j)
184             // y is row vector A(j,j+1:N)
185
186             Subscript ii,jj;
187
188             for (ii=j+1; ii<=M; ii++)
189                 for (jj=j+1; jj<=N; jj++)
190                     A(ii,jj) -= A(ii,j)*A(j,jj);
191         }
192     }
193
194     return 0;
195 }   
196
197
198
199
200 template <class MaTRiX, class VecToR, class VecToRSubscripts>
201 int LU_solve(const MaTRiX &A, const VecToRSubscripts &indx, VecToR &b)
202 {
203     assert(A.lbound() == 1);                // currently for 1-offset
204     assert(indx.lbound() == 1);             // vectors and matrices
205     assert(b.lbound() == 1);
206
207     Subscript i,ii=0,ip,j;
208     Subscript n = b.dim();
209     typename MaTRiX::element_type sum = 0.0;
210
211     for (i=1;i<=n;i++) 
212     {
213         ip=indx(i);
214         sum=b(ip);
215         b(ip)=b(i);
216         if (ii)
217             for (j=ii;j<=i-1;j++) 
218                 sum -= A(i,j)*b(j);
219         else if (sum) ii=i;
220             b(i)=sum;
221     }
222     for (i=n;i>=1;i--) 
223     {
224         sum=b(i);
225         for (j=i+1;j<=n;j++) 
226             sum -= A(i,j)*b(j);
227         b(i)=sum/A(i,i);
228     }
229
230     return 0;
231 }
232
233 } // namespace TNT
234
235 #endif
236 // LU_H