6a4f6e1a5b3829b0d8ce92e4433e70e7fa220053
[blender.git] / intern / iksolver / intern / TNT / trisolve.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 // Triangular Solves
59
60 #ifndef TRISLV_H
61 #define TRISLV_H
62
63
64 #include "triang.h"
65
66 namespace TNT
67 {
68
69 template <class MaTriX, class VecToR>
70 VecToR Lower_triangular_solve(/*const*/ MaTriX &A, /*const*/ VecToR &b)
71 {
72     Subscript N = A.num_rows();
73
74     // make sure matrix sizes agree; A must be square
75
76     assert(A.num_cols() == N);
77     assert(b.dim() == N);
78
79     VecToR x(N);
80
81     Subscript i;
82     for (i=1; i<=N; i++)
83     {
84         typename MaTriX::element_type tmp=0;
85
86         for (Subscript j=1; j<i; j++)
87                 tmp = tmp + A(i,j)*x(j);
88
89         x(i) =  (b(i) - tmp)/ A(i,i);
90     }
91
92     return x;
93 }
94
95
96 template <class MaTriX, class VecToR>
97 VecToR Unit_lower_triangular_solve(/*const*/ MaTriX &A, /*const*/ VecToR &b)
98 {
99     Subscript N = A.num_rows();
100
101     // make sure matrix sizes agree; A must be square
102
103     assert(A.num_cols() == N);
104     assert(b.dim() == N);
105
106     VecToR x(N);
107
108     Subscript i;
109     for (i=1; i<=N; i++)
110     {
111
112         typename MaTriX::element_type tmp=0;
113
114         for (Subscript j=1; j<i; j++)
115                 tmp = tmp + A(i,j)*x(j);
116
117         x(i) =  b(i) - tmp;
118     }
119
120     return x;
121 }
122
123
124 template <class MaTriX, class VecToR>
125 VecToR linear_solve(/*const*/ LowerTriangularView<MaTriX> &A, 
126             /*const*/ VecToR &b)
127 {
128     return Lower_triangular_solve(A, b);
129 }
130     
131 template <class MaTriX, class VecToR>
132 VecToR linear_solve(/*const*/ UnitLowerTriangularView<MaTriX> &A, 
133         /*const*/ VecToR &b)
134 {
135     return Unit_lower_triangular_solve(A, b);
136 }
137     
138
139
140 //********************** Upper triangular section ****************
141
142 template <class MaTriX, class VecToR>
143 VecToR Upper_triangular_solve(/*const*/ MaTriX &A, /*const*/ VecToR &b)
144 {
145     Subscript N = A.num_rows();
146
147     // make sure matrix sizes agree; A must be square
148
149     assert(A.num_cols() == N);
150     assert(b.dim() == N);
151
152     VecToR x(N);
153
154     Subscript i;
155     for (i=N; i>=1; i--)
156     {
157
158         typename MaTriX::element_type tmp=0;
159
160         for (Subscript j=i+1; j<=N; j++)
161                 tmp = tmp + A(i,j)*x(j);
162
163         x(i) =  (b(i) - tmp)/ A(i,i);
164     }
165
166     return x;
167 }
168
169
170 template <class MaTriX, class VecToR>
171 VecToR Unit_upper_triangular_solve(/*const*/ MaTriX &A, /*const*/ VecToR &b)
172 {
173     Subscript N = A.num_rows();
174
175     // make sure matrix sizes agree; A must be square
176
177     assert(A.num_cols() == N);
178     assert(b.dim() == N);
179
180     VecToR x(N);
181
182     Subscript i;
183     for (i=N; i>=1; i--)
184     {
185
186         typename MaTriX::element_type tmp=0;
187
188         for (Subscript j=i+1; j<i; j++)
189                 tmp = tmp + A(i,j)*x(j);
190
191         x(i) =  b(i) - tmp;
192     }
193
194     return x;
195 }
196
197
198 template <class MaTriX, class VecToR>
199 VecToR linear_solve(/*const*/ UpperTriangularView<MaTriX> &A, 
200         /*const*/ VecToR &b)
201 {
202     return Upper_triangular_solve(A, b);
203 }
204     
205 template <class MaTriX, class VecToR>
206 VecToR linear_solve(/*const*/ UnitUpperTriangularView<MaTriX> &A, 
207     /*const*/ VecToR &b)
208 {
209     return Unit_upper_triangular_solve(A, b);
210 }
211
212
213 } // namespace TNT
214
215 #endif
216 // TRISLV_H