diff options
Diffstat (limited to 'intern/iksolver/intern/TNT/lu.h')
| -rw-r--r-- | intern/iksolver/intern/TNT/lu.h | 236 |
1 files changed, 236 insertions, 0 deletions
diff --git a/intern/iksolver/intern/TNT/lu.h b/intern/iksolver/intern/TNT/lu.h new file mode 100644 index 00000000000..b86027aa386 --- /dev/null +++ b/intern/iksolver/intern/TNT/lu.h @@ -0,0 +1,236 @@ +/** + * $Id$ + * ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. The Blender + * Foundation also sells licenses for use in proprietary software under + * the Blender License. See http://www.blender.org/BL/ for information + * about this. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Contributor(s): none yet. + * + * ***** END GPL/BL DUAL LICENSE BLOCK ***** + */ + +/* + +* +* Template Numerical Toolkit (TNT): Linear Algebra Module +* +* Mathematical and Computational Sciences Division +* National Institute of Technology, +* Gaithersburg, MD USA +* +* +* This software was developed at the National Institute of Standards and +* Technology (NIST) by employees of the Federal Government in the course +* of their official duties. Pursuant to title 17 Section 105 of the +* United States Code, this software is not subject to copyright protection +* and is in the public domain. The Template Numerical Toolkit (TNT) is +* an experimental system. NIST assumes no responsibility whatsoever for +* its use by other parties, and makes no guarantees, expressed or implied, +* about its quality, reliability, or any other characteristic. +* +* BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE +* see http://math.nist.gov/tnt for latest updates. +* +*/ + + + +#ifndef LU_H +#define LU_H + +// Solve system of linear equations Ax = b. +// +// Typical usage: +// +// Matrix(double) A; +// Vector(Subscript) ipiv; +// Vector(double) b; +// +// 1) LU_Factor(A,ipiv); +// 2) LU_Solve(A,ipiv,b); +// +// Now b has the solution x. Note that both A and b +// are overwritten. If these values need to be preserved, +// one can make temporary copies, as in +// +// O) Matrix(double) T = A; +// 1) LU_Factor(T,ipiv); +// 1a) Vector(double) x=b; +// 2) LU_Solve(T,ipiv,x); +// +// See details below. +// + + +// for fabs() +// +#include <cmath> + +// right-looking LU factorization algorithm (unblocked) +// +// Factors matrix A into lower and upper triangular matrices +// (L and U respectively) in solving the linear equation Ax=b. +// +// +// Args: +// +// A (input/output) Matrix(1:n, 1:n) In input, matrix to be +// factored. On output, overwritten with lower and +// upper triangular factors. +// +// indx (output) Vector(1:n) Pivot vector. Describes how +// the rows of A were reordered to increase +// numerical stability. +// +// Return value: +// +// int (0 if successful, 1 otherwise) +// +// + + +namespace TNT +{ + +template <class MaTRiX, class VecToRSubscript> +int LU_factor( MaTRiX &A, VecToRSubscript &indx) +{ + assert(A.lbound() == 1); // currently for 1-offset + assert(indx.lbound() == 1); // vectors and matrices + + Subscript M = A.num_rows(); + Subscript N = A.num_cols(); + + if (M == 0 || N==0) return 0; + if (indx.dim() != M) + indx.newsize(M); + + Subscript i=0,j=0,k=0; + Subscript jp=0; + + typename MaTRiX::element_type t; + + Subscript minMN = (M < N ? M : N) ; // min(M,N); + + for (j=1; j<= minMN; j++) + { + + // find pivot in column j and test for singularity. + + jp = j; + t = fabs(A(j,j)); + for (i=j+1; i<=M; i++) + if ( fabs(A(i,j)) > t) + { + jp = i; + t = fabs(A(i,j)); + } + + indx(j) = jp; + + // jp now has the index of maximum element + // of column j, below the diagonal + + if ( A(jp,j) == 0 ) + return 1; // factorization failed because of zero pivot + + + if (jp != j) // swap rows j and jp + for (k=1; k<=N; k++) + { + t = A(j,k); + A(j,k) = A(jp,k); + A(jp,k) =t; + } + + if (j<M) // compute elements j+1:M of jth column + { + // note A(j,j), was A(jp,p) previously which was + // guarranteed not to be zero (Label #1) + // + typename MaTRiX::element_type recp = 1.0 / A(j,j); + + for (k=j+1; k<=M; k++) + A(k,j) *= recp; + } + + + if (j < minMN) + { + // rank-1 update to trailing submatrix: E = E - x*y; + // + // E is the region A(j+1:M, j+1:N) + // x is the column vector A(j+1:M,j) + // y is row vector A(j,j+1:N) + + Subscript ii,jj; + + for (ii=j+1; ii<=M; ii++) + for (jj=j+1; jj<=N; jj++) + A(ii,jj) -= A(ii,j)*A(j,jj); + } + } + + return 0; +} + + + + +template <class MaTRiX, class VecToR, class VecToRSubscripts> +int LU_solve(const MaTRiX &A, const VecToRSubscripts &indx, VecToR &b) +{ + assert(A.lbound() == 1); // currently for 1-offset + assert(indx.lbound() == 1); // vectors and matrices + assert(b.lbound() == 1); + + Subscript i,ii=0,ip,j; + Subscript n = b.dim(); + typename MaTRiX::element_type sum = 0.0; + + for (i=1;i<=n;i++) + { + ip=indx(i); + sum=b(ip); + b(ip)=b(i); + if (ii) + for (j=ii;j<=i-1;j++) + sum -= A(i,j)*b(j); + else if (sum) ii=i; + b(i)=sum; + } + for (i=n;i>=1;i--) + { + sum=b(i); + for (j=i+1;j<=n;j++) + sum -= A(i,j)*b(j); + b(i)=sum/A(i,i); + } + + return 0; +} + +} // namespace TNT + +#endif +// LU_H |