Initial revision
[blender.git] / intern / iksolver / intern / TNT / cholesky.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 CHOLESKY_H
59 #define CHOLESKY_H
60
61 #include <cmath>
62
63 // index method
64
65 namespace TNT
66 {
67
68
69 //
70 // Only upper part of A is used.  Cholesky factor is returned in
71 // lower part of L.  Returns 0 if successful, 1 otherwise.
72 //
73 template <class SPDMatrix, class SymmMatrix>
74 int Cholesky_upper_factorization(SPDMatrix &A, SymmMatrix &L)
75 {
76     Subscript M = A.dim(1);
77     Subscript N = A.dim(2);
78
79     assert(M == N);                 // make sure A is square
80
81     // readjust size of L, if necessary
82
83     if (M != L.dim(1) || N != L.dim(2))
84         L = SymmMatrix(N,N);
85
86     Subscript i,j,k;
87
88
89     typename SPDMatrix::element_type dot=0;
90
91
92     for (j=1; j<=N; j++)                // form column j of L
93     {
94         dot= 0;
95
96         for (i=1; i<j; i++)             // for k= 1 TO j-1
97             dot = dot +  L(j,i)*L(j,i);
98
99         L(j,j) = A(j,j) - dot;
100
101         for (i=j+1; i<=N; i++)
102         {
103             dot = 0;
104             for (k=1; k<j; k++)
105                 dot = dot +  L(i,k)*L(j,k);
106             L(i,j) = A(j,i) - dot;
107         }
108
109         if (L(j,j) <= 0.0) return 1;
110
111         L(j,j) = sqrt( L(j,j) );
112
113         for (i=j+1; i<=N; i++)
114             L(i,j) = L(i,j) / L(j,j);
115
116     }
117
118     return 0;
119 }
120
121
122
123
124 }  
125 // namespace TNT
126
127 #endif
128 // CHOLESKY_H