Initial revision
[blender.git] / intern / iksolver / intern / TNT / qr.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 #ifndef QR_H
58 #define QR_H
59
60 // Classical QR factorization example, based on Stewart[1973].
61 //
62 //
63 // This algorithm computes the factorization of a matrix A
64 // into a product of an orthognal matrix (Q) and an upper triangular 
65 // matrix (R), such that QR = A.
66 //
67 // Parameters:
68 //
69 //  A   (in):       Matrix(1:N, 1:N)
70 //
71 //  Q   (output):   Matrix(1:N, 1:N), collection of Householder
72 //                      column vectors Q1, Q2, ... QN
73 //
74 //  R   (output):   upper triangular Matrix(1:N, 1:N)
75 //
76 // Returns:  
77 //
78 //  0 if successful, 1 if A is detected to be singular
79 //
80
81
82 #include <cmath>      //for sqrt() & fabs()
83 #include "tntmath.h"  // for sign()
84
85 // Classical QR factorization, based on Stewart[1973].
86 //
87 //
88 // This algorithm computes the factorization of a matrix A
89 // into a product of an orthognal matrix (Q) and an upper triangular 
90 // matrix (R), such that QR = A.
91 //
92 // Parameters:
93 //
94 //  A   (in/out):  On input, A is square, Matrix(1:N, 1:N), that represents
95 //                  the matrix to be factored.
96 //
97 //                 On output, Q and R is encoded in the same Matrix(1:N,1:N)
98 //                 in the following manner:
99 //
100 //                  R is contained in the upper triangular section of A,
101 //                  except that R's main diagonal is in D.  The lower
102 //                  triangular section of A represents Q, where each
103 //                  column j is the vector  Qj = I - uj*uj'/pi_j.
104 //
105 //  C  (output):    vector of Pi[j]
106 //  D  (output):    main diagonal of R, i.e. D(i) is R(i,i)
107 //
108 // Returns:  
109 //
110 //  0 if successful, 1 if A is detected to be singular
111 //
112
113 namespace TNT
114 {
115
116 template <class MaTRiX, class Vector>
117 int QR_factor(MaTRiX &A, Vector& C, Vector &D)
118 {
119     assert(A.lbound() == 1);        // ensure these are all 
120     assert(C.lbound() == 1);        // 1-based arrays and vectors
121     assert(D.lbound() == 1);
122
123     Subscript M = A.num_rows();
124     Subscript N = A.num_cols(); 
125
126     assert(M == N);                 // make sure A is square
127
128     Subscript i,j,k;
129     typename MaTRiX::element_type eta, sigma, sum;
130
131     // adjust the shape of C and D, if needed...
132
133     if (N != C.size())  C.newsize(N);
134     if (N != D.size())  D.newsize(N);
135
136     for (k=1; k<N; k++)
137     {
138         // eta = max |M(i,k)|,  for k <= i <= n
139         //
140         eta = 0;
141         for (i=k; i<=N; i++)
142         {
143             double absA = fabs(A(i,k));
144             eta = ( absA >  eta ? absA : eta ); 
145         }
146
147         if (eta == 0)           // matrix is singular
148         {
149             cerr << "QR: k=" << k << "\n";
150             return 1;
151         }
152
153         // form Qk and premiltiply M by it
154         //
155         for(i=k; i<=N; i++)
156             A(i,k)  = A(i,k) / eta;
157
158         sum = 0;
159         for (i=k; i<=N; i++)
160             sum = sum + A(i,k)*A(i,k);
161         sigma = sign(A(k,k)) *  sqrt(sum);
162
163
164         A(k,k) = A(k,k) + sigma;
165         C(k) = sigma * A(k,k);
166         D(k) = -eta * sigma;
167
168         for (j=k+1; j<=N; j++)
169         {
170             sum = 0;
171             for (i=k; i<=N; i++)
172                 sum = sum + A(i,k)*A(i,j);
173             sum = sum / C(k);
174
175             for (i=k; i<=N; i++)
176                 A(i,j) = A(i,j) - sum * A(i,k);
177         }
178
179         D(N) = A(N,N);
180     }
181
182     return 0;
183 }
184
185 // modified form of upper triangular solve, except that the main diagonal
186 // of R (upper portion of A) is in D.
187 //
188 template <class MaTRiX, class Vector>
189 int R_solve(const MaTRiX &A, /*const*/ Vector &D, Vector &b)
190 {
191     assert(A.lbound() == 1);        // ensure these are all 
192     assert(D.lbound() == 1);        // 1-based arrays and vectors
193     assert(b.lbound() == 1);
194
195     Subscript i,j;
196     Subscript N = A.num_rows();
197
198     assert(N == A.num_cols());
199     assert(N == D.dim());
200     assert(N == b.dim());
201
202     typename MaTRiX::element_type sum;
203
204     if (D(N) == 0)
205         return 1;
206
207     b(N) = b(N) / 
208             D(N);
209
210     for (i=N-1; i>=1; i--)
211     {
212         if (D(i) == 0)
213             return 1;
214         sum = 0;
215         for (j=i+1; j<=N; j++)
216             sum = sum + A(i,j)*b(j);
217         b(i) = ( b(i) - sum ) / 
218             D(i);
219     }
220
221     return 0;
222 }
223
224
225 template <class MaTRiX, class Vector>
226 int QR_solve(const MaTRiX &A, const Vector &c, /*const*/ Vector &d, 
227         Vector &b)
228 {
229     assert(A.lbound() == 1);        // ensure these are all 
230     assert(c.lbound() == 1);        // 1-based arrays and vectors
231     assert(d.lbound() == 1);
232
233     Subscript N=A.num_rows();
234
235     assert(N == A.num_cols());
236     assert(N == c.dim());
237     assert(N == d.dim());
238     assert(N == b.dim());
239
240     Subscript i,j;
241     typename MaTRiX::element_type sum, tau;
242
243     for (j=1; j<N; j++)
244     {
245         // form Q'*b
246         sum = 0;
247         for (i=j; i<=N; i++)
248             sum = sum + A(i,j)*b(i);
249         if (c(j) == 0)
250             return 1;
251         tau = sum / c(j);
252        for (i=j; i<=N; i++)
253             b(i) = b(i) - tau * A(i,j);
254     }
255     return R_solve(A, d, b);        // solve Rx = Q'b
256 }
257
258 } // namespace TNT
259
260 #endif
261 // QR_H