X-Git-Url: https://git.dogcows.com/gitweb?p=chaz%2Fyoink;a=blobdiff_plain;f=src%2FMoof%2Fcml%2Fmatrix%2Finverse.h;fp=src%2FMoof%2Fcml%2Fmatrix%2Finverse.h;h=4d80e6f40382c2f381926715c4db67e821f3a318;hp=0000000000000000000000000000000000000000;hb=c2321281bf12a7efaedde930422c7ddbc92080d4;hpb=87bc17e55b0c1dc73ecc66df856d3f08fd7a7724 diff --git a/src/Moof/cml/matrix/inverse.h b/src/Moof/cml/matrix/inverse.h new file mode 100644 index 0000000..4d80e6f --- /dev/null +++ b/src/Moof/cml/matrix/inverse.h @@ -0,0 +1,443 @@ +/* -*- C++ -*- ------------------------------------------------------------ + +Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/ + +The Configurable Math Library (CML) is distributed under the terms of the +Boost Software License, v1.0 (see cml/LICENSE for details). + + *-----------------------------------------------------------------------*/ +/** @file + * @brief Compute the inverse of a matrix by LU factorization. + */ + +#ifndef matrix_inverse_h +#define matrix_inverse_h + +#include + +namespace cml { +namespace detail { + +/* Need to use a functional, since template functions cannot be + * specialized. _tag is used to specialize based upon dimension: + */ +template struct inverse_f; + +/* @todo: Reciprocal optimization for division by determinant. + */ + +/* 2x2 inverse. Despite being marked for fixed_size matrices, this can + * be used for dynamic-sized ones also: + */ +template +struct inverse_f +{ + typename MatT::temporary_type operator()(const MatT& M) const + { + typedef typename MatT::temporary_type temporary_type; + typedef typename temporary_type::value_type value_type; + + /* Matrix containing the inverse: */ + temporary_type Z; + cml::et::detail::Resize(Z,2,2); + + /* Compute determinant and inverse: */ + value_type D = value_type(1) / (M(0,0)*M(1,1) - M(0,1)*M(1,0)); + Z(0,0) = M(1,1)*D; Z(0,1) = - M(0,1)*D; + Z(1,0) = - M(1,0)*D; Z(1,1) = M(0,0)*D; + + return Z; + } +}; + +/* 3x3 inverse. Despite being marked for fixed_size matrices, this can + * be used for dynamic-sized ones also: + */ +template +struct inverse_f +{ + /* [00 01 02] + * M = [10 11 12] + * [20 21 22] + */ + typename MatT::temporary_type operator()(const MatT& M) const + { + /* Shorthand. */ + typedef typename MatT::value_type value_type; + + /* Compute cofactors for each entry: */ + value_type m_00 = M(1,1)*M(2,2) - M(1,2)*M(2,1); + value_type m_01 = M(1,2)*M(2,0) - M(1,0)*M(2,2); + value_type m_02 = M(1,0)*M(2,1) - M(1,1)*M(2,0); + + value_type m_10 = M(0,2)*M(2,1) - M(0,1)*M(2,2); + value_type m_11 = M(0,0)*M(2,2) - M(0,2)*M(2,0); + value_type m_12 = M(0,1)*M(2,0) - M(0,0)*M(2,1); + + value_type m_20 = M(0,1)*M(1,2) - M(0,2)*M(1,1); + value_type m_21 = M(0,2)*M(1,0) - M(0,0)*M(1,2); + value_type m_22 = M(0,0)*M(1,1) - M(0,1)*M(1,0); + + /* Compute determinant from the minors: */ + value_type D = + value_type(1) / (M(0,0)*m_00 + M(0,1)*m_01 + M(0,2)*m_02); + + /* Matrix containing the inverse: */ + typename MatT::temporary_type Z; + cml::et::detail::Resize(Z,3,3); + + /* Assign the inverse as (1/D) * (cofactor matrix)^T: */ + Z(0,0) = m_00*D; Z(0,1) = m_10*D; Z(0,2) = m_20*D; + Z(1,0) = m_01*D; Z(1,1) = m_11*D; Z(1,2) = m_21*D; + Z(2,0) = m_02*D; Z(2,1) = m_12*D; Z(2,2) = m_22*D; + + return Z; + } +}; + +/* 4x4 inverse. Despite being marked for fixed_size matrices, this can + * be used for dynamic-sized ones also: + */ +template +struct inverse_f +{ + /* [00 01 02 03] + * M = [10 11 12 13] + * [20 21 22 23] + * [30 31 32 33] + * + * |11 12 13| |10 12 13| + * C00 = |21 22 23| C01 = |20 22 23| + * |31 32 33| |30 32 33| + * + * |10 11 13| |10 11 12| + * C02 = |20 21 23| C03 = |20 21 22| + * |30 31 33| |30 31 32| + */ + typename MatT::temporary_type operator()(const MatT& M) const + { + /* Shorthand. */ + typedef typename MatT::value_type value_type; + + /* Common cofactors, rows 0,1: */ + value_type m_22_33_23_32 = M(2,2)*M(3,3) - M(2,3)*M(3,2); + value_type m_23_30_20_33 = M(2,3)*M(3,0) - M(2,0)*M(3,3); + value_type m_20_31_21_30 = M(2,0)*M(3,1) - M(2,1)*M(3,0); + value_type m_21_32_22_31 = M(2,1)*M(3,2) - M(2,2)*M(3,1); + value_type m_23_31_21_33 = M(2,3)*M(3,1) - M(2,1)*M(3,3); + value_type m_20_32_22_30 = M(2,0)*M(3,2) - M(2,2)*M(3,0); + + /* Compute minors: */ + value_type d00 + = M(1,1)*m_22_33_23_32+M(1,2)*m_23_31_21_33+M(1,3)*m_21_32_22_31; + + value_type d01 + = M(1,0)*m_22_33_23_32+M(1,2)*m_23_30_20_33+M(1,3)*m_20_32_22_30; + + value_type d02 + = M(1,0)*-m_23_31_21_33+M(1,1)*m_23_30_20_33+M(1,3)*m_20_31_21_30; + + value_type d03 + = M(1,0)*m_21_32_22_31+M(1,1)*-m_20_32_22_30+M(1,2)*m_20_31_21_30; + + /* Compute minors: */ + value_type d10 + = M(0,1)*m_22_33_23_32+M(0,2)*m_23_31_21_33+M(0,3)*m_21_32_22_31; + + value_type d11 + = M(0,0)*m_22_33_23_32+M(0,2)*m_23_30_20_33+M(0,3)*m_20_32_22_30; + + value_type d12 + = M(0,0)*-m_23_31_21_33+M(0,1)*m_23_30_20_33+M(0,3)*m_20_31_21_30; + + value_type d13 + = M(0,0)*m_21_32_22_31+M(0,1)*-m_20_32_22_30+M(0,2)*m_20_31_21_30; + + /* Common cofactors, rows 2,3: */ + value_type m_02_13_03_12 = M(0,2)*M(1,3) - M(0,3)*M(1,2); + value_type m_03_10_00_13 = M(0,3)*M(1,0) - M(0,0)*M(1,3); + value_type m_00_11_01_10 = M(0,0)*M(1,1) - M(0,1)*M(1,0); + value_type m_01_12_02_11 = M(0,1)*M(1,2) - M(0,2)*M(1,1); + value_type m_03_11_01_13 = M(0,3)*M(1,1) - M(0,1)*M(1,3); + value_type m_00_12_02_10 = M(0,0)*M(1,2) - M(0,2)*M(1,0); + + /* Compute minors (uses row 3 as the multipliers instead of row 0, + * which uses the same signs as row 0): + */ + value_type d20 + = M(3,1)*m_02_13_03_12+M(3,2)*m_03_11_01_13+M(3,3)*m_01_12_02_11; + + value_type d21 + = M(3,0)*m_02_13_03_12+M(3,2)*m_03_10_00_13+M(3,3)*m_00_12_02_10; + + value_type d22 + = M(3,0)*-m_03_11_01_13+M(3,1)*m_03_10_00_13+M(3,3)*m_00_11_01_10; + + value_type d23 + = M(3,0)*m_01_12_02_11+M(3,1)*-m_00_12_02_10+M(3,2)*m_00_11_01_10; + + /* Compute minors: */ + value_type d30 + = M(2,1)*m_02_13_03_12+M(2,2)*m_03_11_01_13+M(2,3)*m_01_12_02_11; + + value_type d31 + = M(2,0)*m_02_13_03_12+M(2,2)*m_03_10_00_13+M(2,3)*m_00_12_02_10; + + value_type d32 + = M(2,0)*-m_03_11_01_13+M(2,1)*m_03_10_00_13+M(2,3)*m_00_11_01_10; + + value_type d33 + = M(2,0)*m_01_12_02_11+M(2,1)*-m_00_12_02_10+M(2,2)*m_00_11_01_10; + + /* Finally, compute determinant from the minors, and assign the + * inverse as (1/D) * (cofactor matrix)^T: + */ + typename MatT::temporary_type Z; + cml::et::detail::Resize(Z,4,4); + + value_type D = value_type(1) / + (M(0,0)*d00 - M(0,1)*d01 + M(0,2)*d02 - M(0,3)*d03); + Z(0,0) = +d00*D; Z(0,1) = -d10*D; Z(0,2) = +d20*D; Z(0,3) = -d30*D; + Z(1,0) = -d01*D; Z(1,1) = +d11*D; Z(1,2) = -d21*D; Z(1,3) = +d31*D; + Z(2,0) = +d02*D; Z(2,1) = -d12*D; Z(2,2) = +d22*D; Z(2,3) = -d32*D; + Z(3,0) = -d03*D; Z(3,1) = +d13*D; Z(3,2) = -d23*D; Z(3,3) = +d33*D; + + return Z; + } +}; + +/* If more extensive general linear algebra functionality is offered in + * future versions it may be useful to make the elementary row and column + * operations separate functions. For now they're simply performed in place, + * but the commented-out lines of code show where the calls to these functions + * should go if and when they become available. + */ + +/* @todo: In-place version, and address memory allocation for pivot vector. + */ + +/* General NxN inverse by Gauss-Jordan elimination with full pivoting: */ +template +struct inverse_f +{ + typename MatT::temporary_type operator()(const MatT& M) const + { + /* Shorthand. */ + typedef typename MatT::value_type value_type; + + /* Size of matrix */ + size_t N = M.rows(); + + /* Matrix containing the inverse: */ + typename MatT::temporary_type Z; + cml::et::detail::Resize(Z,N,N); + Z = M; + + /* For tracking pivots */ + std::vector row_index(N); + std::vector col_index(N); + std::vector pivoted(N,0); + + /* For each column */ + for (size_t i = 0; i < N; ++i) { + + /* Find the pivot */ + size_t row = 0, col = 0; + value_type max = value_type(0); + for (size_t j = 0; j < N; ++j) { + if (!pivoted[j]) { + for (size_t k = 0; k < N; ++k) { + if (!pivoted[k]) { + value_type mag = std::fabs(Z(j,k)); + if (mag > max) { + max = mag; + row = j; + col = k; + } + } + } + } + } + + /* TODO: Check max against epsilon here to catch singularity */ + + row_index[i] = row; + col_index[i] = col; + + /* Swap rows if necessary */ + if (row != col) { + /*Z.row_op_swap(row,col);*/ + for (size_t j = 0; j < Z.cols(); ++j) { + std::swap(Z(row,j),Z(col,j)); + } + } + + /* Process pivot row */ + pivoted[col] = true; + value_type pivot = Z(col,col); + Z(col,col) = value_type(1); + /*Z.row_op_mult(col,value_type(1)/pivot);*/ + value_type k = value_type(1)/pivot; + for (size_t j = 0; j < Z.cols(); ++j) { + Z(col,j) *= k; + } + + /* Process other rows */ + for (size_t j = 0; j < N; ++j) { + if (j != col) { + value_type mult = -Z(j,col); + Z(j,col) = value_type(0); + /*Z.row_op_add_mult(col,j,mult);*/ + for (size_t k = 0; k < Z.cols(); ++k) { + Z(j,k) += mult * Z(col,k); + } + } + } + } + + /* Swap columns if necessary */ + for (int i = N-1; i >= 0; --i) { + if (row_index[i] != col_index[i]) { + /*Z.col_op_swap(row_index[i],col_index[i]);*/ + for (size_t j = 0; j < Z.rows(); ++j) { + std::swap(Z(j,row_index[i]),Z(j,col_index[i])); + } + } + } + + /* Return result */ + return Z; + } +}; + +/* Inversion by LU factorization is turned off for now due to lack of + * pivoting in the implementation, but we may switch back to it at some future + * time. + */ + +#if 0 + +/* General NxN inverse by LU factorization: */ +template +struct inverse_f +{ + typename MatT::temporary_type operator()(const MatT& M) const + { + /* Shorthand. */ + typedef typename MatT::value_type value_type; + + /* Compute LU factorization: */ + size_t N = M.rows(); + typename MatT::temporary_type LU; + cml::et::detail::Resize(LU,N,N); + LU = lu(M); + + /* Matrix containing the inverse: */ + typename MatT::temporary_type Z; + cml::et::detail::Resize(Z,N,N); + + typename MatT::col_vector_type v, x; + cml::et::detail::Resize(v,N); + cml::et::detail::Resize(x,N); + for(size_t i = 0; i < N; ++i) + v[i] = value_type(0); + /* XXX Need a fill() function here. */ + + /* Use lu_solve to solve M*x = v for x, where v = [0 ... 1 ... 0]^T: */ + for(size_t i = 0; i < N; ++i) { + v[i] = 1.; + x = lu_solve(LU,v); + + /* x is column i of the inverse of LU: */ + for(size_t k = 0; k < N; ++ k) { + Z(k,i) = x[k]; + } + v[i] = 0.; + } + + return Z; + } + +}; + +#endif + +/* Note: force_NxN is for checking general NxN inversion against the special- + * case 2x2, 3x3 and 4x4 code. I'm leaving it in for now since we may need to + * test the NxN code further if the implementation changes. At some future + * time when the implementation is stable, everything related to force_NxN can + * be taken out. + */ + +/* Note: Commenting the force_NxN stuff out, but leaving the code here in + * case we need to do more testing in the future. + */ + +/* Generator for the inverse functional for fixed-size matrices: */ +template typename MatT::temporary_type +inverse(const MatT& M, fixed_size_tag/*, bool force_NxN*/) +{ + /* Require a square matrix: */ + cml::et::CheckedSquare(M, fixed_size_tag()); + + /* + if (force_NxN) { + return inverse_f()(M); + } else { + */ + return inverse_f()(M); + /* + } + */ +} + +/* Generator for the inverse functional for dynamic-size matrices: */ +template typename MatT::temporary_type +inverse(const MatT& M, dynamic_size_tag/*, bool force_NxN*/) +{ + /* Require a square matrix: */ + cml::et::CheckedSquare(M, dynamic_size_tag()); + + /* + if (force_NxN) { + return inverse_f()(M); + } else { + */ + /* Dispatch based upon the matrix dimension: */ + switch(M.rows()) { + case 2: return inverse_f()(M); // 2x2 + case 3: return inverse_f()(M); // 3x3 + case 4: return inverse_f()(M); // 4x4 + default: return inverse_f()(M); // > 4x4 (or 1x1) + } + /* + } + */ +} + +} // namespace detail + +/** Inverse of a matrix. */ +template inline +typename matrix::temporary_type +inverse(const matrix& M/*, bool force_NxN = false*/) +{ + typedef typename matrix::size_tag size_tag; + return detail::inverse(M,size_tag()/*,force_NxN*/); +} + +/** Inverse of a matrix expression. */ +template inline +typename et::MatrixXpr::temporary_type +inverse(const et::MatrixXpr& e/*, bool force_NxN = false*/) +{ + typedef typename et::MatrixXpr::size_tag size_tag; + return detail::inverse(e,size_tag()/*,force_NxN*/); +} + +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp