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=0000000000000000000000000000000000000000;hp=359a42c2480c5f516ea9861d4de9294b164a52cb;hb=831f04d4bc19a390415ac0bbac4331c7a65509bc;hpb=299af4f2047e767e5d79501c26444473bda64c64 diff --git a/src/Moof/cml/matrix/inverse.h b/src/Moof/cml/matrix/inverse.h deleted file mode 100644 index 359a42c..0000000 --- a/src/Moof/cml/matrix/inverse.h +++ /dev/null @@ -1,444 +0,0 @@ -/* -*- 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 -#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