OpenWalnut  1.2.5
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
WLinearAlgebraFunctions.cpp
00001 //---------------------------------------------------------------------------
00002 //
00003 // Project: OpenWalnut ( http://www.openwalnut.org )
00004 //
00005 // Copyright 2009 OpenWalnut Community, BSV@Uni-Leipzig and CNCF@MPI-CBS
00006 // For more information see http://www.openwalnut.org/copying
00007 //
00008 // This file is part of OpenWalnut.
00009 //
00010 // OpenWalnut is free software: you can redistribute it and/or modify
00011 // it under the terms of the GNU Lesser General Public License as published by
00012 // the Free Software Foundation, either version 3 of the License, or
00013 // (at your option) any later version.
00014 //
00015 // OpenWalnut is distributed in the hope that it will be useful,
00016 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00017 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018 // GNU Lesser General Public License for more details.
00019 //
00020 // You should have received a copy of the GNU Lesser General Public License
00021 // along with OpenWalnut. If not, see <http://www.gnu.org/licenses/>.
00022 //
00023 //---------------------------------------------------------------------------
00024 
00025 #include <vector>
00026 
00027 #include <Eigen/SVD>
00028 
00029 #include "../WAssert.h"
00030 #include "../WLimits.h"
00031 
00032 #include "WLinearAlgebraFunctions.h"
00033 #include "WMatrix.h"
00034 #include "linearAlgebra/WLinearAlgebra.h"
00035 
00036 WVector3d multMatrixWithVector3D( WMatrix<double> mat, WVector3d vec )
00037 {
00038     WVector3d result;
00039     result[0] = mat( 0, 0 ) * vec[0] + mat( 0, 1 ) * vec[1] + mat( 0, 2 ) * vec[2];
00040     result[1] = mat( 1, 0 ) * vec[0] + mat( 1, 1 ) * vec[1] + mat( 1, 2 ) * vec[2];
00041     result[2] = mat( 2, 0 ) * vec[0] + mat( 2, 1 ) * vec[1] + mat( 2, 2 ) * vec[2];
00042     return result;
00043 }
00044 
00045 WVector3d transformVector3DWithMatrix4D( WMatrix<double> mat, WVector3d vec )
00046 {
00047     WAssert( mat.getNbRows() == 4 && mat.getNbCols() == 4, "Matrix has wrong size." );
00048     std::vector< double > resultVec4D( 4 );
00049     resultVec4D[0] = mat( 0, 0 ) * vec[0] + mat( 0, 1 ) * vec[1] + mat( 0, 2 ) * vec[2] /* + mat( 0, 3 ) * 0 */;
00050     resultVec4D[1] = mat( 1, 0 ) * vec[0] + mat( 1, 1 ) * vec[1] + mat( 1, 2 ) * vec[2] /* + mat( 1, 3 ) * 0 */;
00051     resultVec4D[2] = mat( 2, 0 ) * vec[0] + mat( 2, 1 ) * vec[1] + mat( 2, 2 ) * vec[2] /* + mat( 2, 3 ) * 0 */;
00052     resultVec4D[3] = mat( 3, 0 ) * vec[0] + mat( 3, 1 ) * vec[1] + mat( 3, 2 ) * vec[2] /* + mat( 3, 3 ) * 0 */;
00053 
00054     WVector3d result;
00055     result[0] = resultVec4D[0] / resultVec4D[3];
00056     result[1] = resultVec4D[1] / resultVec4D[3];
00057     result[2] = resultVec4D[2] / resultVec4D[3];
00058     return result;
00059 }
00060 
00061 WPosition transformPosition3DWithMatrix4D( WMatrix<double> mat, WPosition vec )
00062 {
00063     WAssert( mat.getNbRows() == 4 && mat.getNbCols() == 4, "Matrix has wrong size." );
00064     std::vector< double > resultVec4D( 4 );
00065     resultVec4D[0] = mat( 0, 0 ) * vec[0] + mat( 0, 1 ) * vec[1] + mat( 0, 2 ) * vec[2] + mat( 0, 3 ) * 1;
00066     resultVec4D[1] = mat( 1, 0 ) * vec[0] + mat( 1, 1 ) * vec[1] + mat( 1, 2 ) * vec[2] + mat( 1, 3 ) * 1;
00067     resultVec4D[2] = mat( 2, 0 ) * vec[0] + mat( 2, 1 ) * vec[1] + mat( 2, 2 ) * vec[2] + mat( 2, 3 ) * 1;
00068     resultVec4D[3] = mat( 3, 0 ) * vec[0] + mat( 3, 1 ) * vec[1] + mat( 3, 2 ) * vec[2] + mat( 3, 3 ) * 1;
00069 
00070     WPosition result;
00071     result[0] = resultVec4D[0] / resultVec4D[3];
00072     result[1] = resultVec4D[1] / resultVec4D[3];
00073     result[2] = resultVec4D[2] / resultVec4D[3];
00074     return result;
00075 }
00076 
00077 WMatrix<double> invertMatrix3x3( WMatrix<double> mat )
00078 {
00079     WAssert( mat.getNbRows(), "Zero rows found." );
00080     WAssert( mat.getNbCols(), "Zero columns found." );
00081     double det = mat( 0, 0 ) * mat( 1, 1 ) * mat( 2, 2 ) +
00082                 mat( 0, 1 ) * mat( 1, 2 ) * mat( 2, 0 ) +
00083                 mat( 0, 2 ) * mat( 1, 0 ) * mat( 2, 1 ) -
00084                 mat( 0, 2 ) * mat( 1, 1 ) * mat( 2, 0 ) -
00085                 mat( 0, 1 ) * mat( 1, 0 ) * mat( 2, 2 ) -
00086                 mat( 0, 0 ) * mat( 1, 2 ) * mat( 2, 1 );
00087 
00088     WAssert( det != 0, "Determinant is zero. This matrix can not be inverted." );
00089 
00090     WMatrix<double> r( 3, 3 );
00091 
00092     r( 0, 0 ) = ( mat( 1, 1 ) * mat( 2, 2 ) - mat(  1, 2 ) * mat( 2, 1 ) ) / det;
00093     r( 1, 0 ) = ( mat( 1, 2 ) * mat( 2, 0 ) - mat(  1, 0 ) * mat( 2, 2 ) ) / det;
00094     r( 2, 0 ) = ( mat( 1, 0 ) * mat( 2, 1 ) - mat(  1, 1 ) * mat( 2, 0 ) ) / det;
00095     r( 0, 1 ) = ( mat( 0, 2 ) * mat( 2, 1 ) - mat(  0, 1 ) * mat( 2, 2 ) ) / det;
00096     r( 1, 1 ) = ( mat( 0, 0 ) * mat( 2, 2 ) - mat(  0, 2 ) * mat( 2, 0 ) ) / det;
00097     r( 2, 1 ) = ( mat( 0, 1 ) * mat( 2, 0 ) - mat(  0, 0 ) * mat( 2, 1 ) ) / det;
00098     r( 0, 2 ) = ( mat( 0, 1 ) * mat( 1, 2 ) - mat(  0, 2 ) * mat( 1, 1 ) ) / det;
00099     r( 1, 2 ) = ( mat( 0, 2 ) * mat( 1, 0 ) - mat(  0, 0 ) * mat( 1, 2 ) ) / det;
00100     r( 2, 2 ) = ( mat( 0, 0 ) * mat( 1, 1 ) - mat(  0, 1 ) * mat( 1, 0 ) ) / det;
00101 
00102     return r;
00103 }
00104 
00105 WMatrix<double> invertMatrix4x4( WMatrix<double> mat )
00106 {
00107     WAssert( mat.getNbRows(), "Zero rows found." );
00108     WAssert( mat.getNbCols(), "Zero columns found." );
00109     double det =
00110         mat( 0, 0 ) * mat( 1, 1 ) * mat( 2, 2 ) * mat( 3, 3 ) +
00111         mat( 0, 0 ) * mat( 1, 2 ) * mat( 2, 3 ) * mat( 3, 1 ) +
00112         mat( 0, 0 ) * mat( 1, 3 ) * mat( 2, 1 ) * mat( 3, 2 ) +
00113 
00114         mat( 0, 1 ) * mat( 1, 0 ) * mat( 2, 3 ) * mat( 3, 2 ) +
00115         mat( 0, 1 ) * mat( 1, 2 ) * mat( 2, 0 ) * mat( 3, 3 ) +
00116         mat( 0, 1 ) * mat( 1, 3 ) * mat( 2, 2 ) * mat( 3, 0 ) +
00117 
00118         mat( 0, 2 ) * mat( 1, 0 ) * mat( 2, 1 ) * mat( 3, 3 ) +
00119         mat( 0, 2 ) * mat( 1, 1 ) * mat( 2, 3 ) * mat( 3, 0 ) +
00120         mat( 0, 2 ) * mat( 1, 3 ) * mat( 2, 0 ) * mat( 3, 1 ) +
00121 
00122         mat( 0, 3 ) * mat( 1, 0 ) * mat( 2, 2 ) * mat( 3, 1 ) +
00123         mat( 0, 3 ) * mat( 1, 1 ) * mat( 2, 0 ) * mat( 3, 2 ) +
00124         mat( 0, 3 ) * mat( 1, 2 ) * mat( 2, 1 ) * mat( 3, 0 ) -
00125 
00126         mat( 0, 0 ) * mat( 1, 1 ) * mat( 2, 3 ) * mat( 3, 2 ) -
00127         mat( 0, 0 ) * mat( 1, 2 ) * mat( 2, 1 ) * mat( 3, 3 ) -
00128         mat( 0, 0 ) * mat( 1, 3 ) * mat( 2, 2 ) * mat( 3, 1 ) -
00129 
00130         mat( 0, 1 ) * mat( 1, 0 ) * mat( 2, 2 ) * mat( 3, 3 ) -
00131         mat( 0, 1 ) * mat( 1, 2 ) * mat( 2, 3 ) * mat( 3, 0 ) -
00132         mat( 0, 1 ) * mat( 1, 3 ) * mat( 2, 0 ) * mat( 3, 2 ) -
00133 
00134         mat( 0, 2 ) * mat( 1, 0 ) * mat( 2, 3 ) * mat( 3, 1 ) -
00135         mat( 0, 2 ) * mat( 1, 1 ) * mat( 2, 0 ) * mat( 3, 3 ) -
00136         mat( 0, 2 ) * mat( 1, 3 ) * mat( 2, 1 ) * mat( 3, 0 ) -
00137 
00138         mat( 0, 3 ) * mat( 1, 0 ) * mat( 2, 1 ) * mat( 3, 2 ) -
00139         mat( 0, 3 ) * mat( 1, 1 ) * mat( 2, 2 ) * mat( 3, 0 ) -
00140         mat( 0, 3 ) * mat( 1, 2 ) * mat( 2, 0 ) * mat( 3, 1 );
00141 
00142     WMatrix<double> result( 4, 4 );
00143 
00144     result( 0, 0 ) =
00145         mat( 1, 1 ) * mat( 2, 2 ) * mat( 3, 3 ) +
00146         mat( 1, 2 ) * mat( 2, 3 ) * mat( 3, 1 ) +
00147         mat( 1, 3 ) * mat( 2, 1 ) * mat( 3, 2 ) -
00148         mat( 1, 1 ) * mat( 2, 3 ) * mat( 3, 2 ) -
00149         mat( 1, 2 ) * mat( 2, 1 ) * mat( 3, 3 ) -
00150         mat( 1, 3 ) * mat( 2, 2 ) * mat( 3, 1 );
00151 
00152     result( 0, 1 ) =
00153         mat( 0, 1 ) * mat( 2, 3 ) * mat( 3, 2 ) +
00154         mat( 0, 2 ) * mat( 2, 1 ) * mat( 3, 3 ) +
00155         mat( 0, 3 ) * mat( 2, 2 ) * mat( 3, 1 ) -
00156         mat( 0, 1 ) * mat( 2, 2 ) * mat( 3, 3 ) -
00157         mat( 0, 2 ) * mat( 2, 3 ) * mat( 3, 1 ) -
00158         mat( 0, 3 ) * mat( 2, 1 ) * mat( 3, 2 );
00159 
00160     result( 0, 2 ) =
00161         mat( 0, 1 ) * mat( 1, 2 ) * mat( 3, 3 ) +
00162         mat( 0, 2 ) * mat( 1, 3 ) * mat( 3, 1 ) +
00163         mat( 0, 3 ) * mat( 1, 1 ) * mat( 3, 2 ) -
00164         mat( 0, 1 ) * mat( 1, 3 ) * mat( 3, 2 ) -
00165         mat( 0, 2 ) * mat( 1, 1 ) * mat( 3, 3 ) -
00166         mat( 0, 3 ) * mat( 1, 2 ) * mat( 3, 1 );
00167 
00168     result( 0, 3 ) =
00169         mat( 0, 1 ) * mat( 1, 3 ) * mat( 2, 2 ) +
00170         mat( 0, 2 ) * mat( 1, 1 ) * mat( 2, 3 ) +
00171         mat( 0, 3 ) * mat( 1, 2 ) * mat( 2, 1 ) -
00172         mat( 0, 1 ) * mat( 1, 2 ) * mat( 2, 3 ) -
00173         mat( 0, 2 ) * mat( 1, 3 ) * mat( 2, 1 ) -
00174         mat( 0, 3 ) * mat( 1, 1 ) * mat( 2, 2 );
00175 
00176     result( 1, 0 ) =
00177         mat( 1, 0 ) * mat( 2, 3 ) * mat( 3, 2 ) +
00178         mat( 1, 2 ) * mat( 2, 0 ) * mat( 3, 3 ) +
00179         mat( 1, 3 ) * mat( 2, 2 ) * mat( 3, 0 ) -
00180         mat( 1, 0 ) * mat( 2, 2 ) * mat( 3, 3 ) -
00181         mat( 1, 2 ) * mat( 2, 3 ) * mat( 3, 0 ) -
00182         mat( 1, 3 ) * mat( 2, 0 ) * mat( 3, 2 );
00183 
00184     result( 1, 1 ) =
00185         mat( 0, 0 ) * mat( 2, 2 ) * mat( 3, 3 ) +
00186         mat( 0, 2 ) * mat( 2, 3 ) * mat( 3, 0 ) +
00187         mat( 0, 3 ) * mat( 2, 0 ) * mat( 3, 2 ) -
00188         mat( 0, 0 ) * mat( 2, 3 ) * mat( 3, 2 ) -
00189         mat( 0, 2 ) * mat( 2, 0 ) * mat( 3, 3 ) -
00190         mat( 0, 3 ) * mat( 2, 2 ) * mat( 3, 0 );
00191 
00192     result( 1, 2 ) =
00193         mat( 0, 0 ) * mat( 1, 3 ) * mat( 3, 2 ) +
00194         mat( 0, 2 ) * mat( 1, 0 ) * mat( 3, 3 ) +
00195         mat( 0, 3 ) * mat( 1, 2 ) * mat( 3, 0 ) -
00196         mat( 0, 0 ) * mat( 1, 2 ) * mat( 3, 3 ) -
00197         mat( 0, 2 ) * mat( 1, 3 ) * mat( 3, 0 ) -
00198         mat( 0, 3 ) * mat( 1, 0 ) * mat( 3, 2 );
00199 
00200     result( 1, 3 ) =
00201         mat( 0, 0 ) * mat( 1, 2 ) * mat( 2, 3 ) +
00202         mat( 0, 2 ) * mat( 1, 3 ) * mat( 2, 0 ) +
00203         mat( 0, 3 ) * mat( 1, 0 ) * mat( 2, 2 ) -
00204         mat( 0, 0 ) * mat( 1, 3 ) * mat( 2, 2 ) -
00205         mat( 0, 2 ) * mat( 1, 0 ) * mat( 2, 3 ) -
00206         mat( 0, 3 ) * mat( 1, 2 ) * mat( 2, 0 );
00207 
00208     result( 2, 0 ) =
00209         mat( 1, 0 ) * mat( 2, 1 ) * mat( 3, 3 ) +
00210         mat( 1, 1 ) * mat( 2, 3 ) * mat( 3, 0 ) +
00211         mat( 1, 3 ) * mat( 2, 0 ) * mat( 3, 1 ) -
00212         mat( 1, 0 ) * mat( 2, 3 ) * mat( 3, 1 ) -
00213         mat( 1, 1 ) * mat( 2, 0 ) * mat( 3, 3 ) -
00214         mat( 1, 3 ) * mat( 2, 1 ) * mat( 3, 0 );
00215 
00216     result( 2, 1 ) =
00217         mat( 0, 0 ) * mat( 2, 3 ) * mat( 3, 1 ) +
00218         mat( 0, 1 ) * mat( 2, 0 ) * mat( 3, 3 ) +
00219         mat( 0, 3 ) * mat( 2, 1 ) * mat( 3, 0 ) -
00220         mat( 0, 0 ) * mat( 2, 1 ) * mat( 3, 3 ) -
00221         mat( 0, 1 ) * mat( 2, 3 ) * mat( 3, 0 ) -
00222         mat( 0, 3 ) * mat( 2, 0 ) * mat( 3, 1 );
00223 
00224     result( 2, 2 ) =
00225         mat( 0, 0 ) * mat( 1, 1 ) * mat( 3, 3 ) +
00226         mat( 0, 1 ) * mat( 1, 3 ) * mat( 3, 0 ) +
00227         mat( 0, 3 ) * mat( 1, 0 ) * mat( 3, 1 ) -
00228         mat( 0, 0 ) * mat( 1, 3 ) * mat( 3, 1 ) -
00229         mat( 0, 1 ) * mat( 1, 0 ) * mat( 3, 3 ) -
00230         mat( 0, 3 ) * mat( 1, 1 ) * mat( 3, 0 );
00231 
00232     result( 2, 3 ) =
00233         mat( 0, 0 ) * mat( 1, 3 ) * mat( 2, 1 ) +
00234         mat( 0, 1 ) * mat( 1, 0 ) * mat( 2, 3 ) +
00235         mat( 0, 3 ) * mat( 1, 1 ) * mat( 2, 0 ) -
00236         mat( 0, 0 ) * mat( 1, 1 ) * mat( 2, 3 ) -
00237         mat( 0, 1 ) * mat( 1, 3 ) * mat( 2, 0 ) -
00238         mat( 0, 3 ) * mat( 1, 0 ) * mat( 2, 1 );
00239 
00240     result( 3, 0 ) =
00241         mat( 1, 0 ) * mat( 2, 2 ) * mat( 3, 1 ) +
00242         mat( 1, 1 ) * mat( 2, 0 ) * mat( 3, 2 ) +
00243         mat( 1, 2 ) * mat( 2, 1 ) * mat( 3, 0 ) -
00244         mat( 1, 0 ) * mat( 2, 1 ) * mat( 3, 2 ) -
00245         mat( 1, 1 ) * mat( 2, 2 ) * mat( 3, 0 ) -
00246         mat( 1, 2 ) * mat( 2, 0 ) * mat( 3, 1 );
00247 
00248     result( 3, 1 ) =
00249         mat( 0, 0 ) * mat( 2, 1 ) * mat( 3, 2 ) +
00250         mat( 0, 1 ) * mat( 2, 2 ) * mat( 3, 0 ) +
00251         mat( 0, 2 ) * mat( 2, 0 ) * mat( 3, 1 ) -
00252         mat( 0, 0 ) * mat( 2, 2 ) * mat( 3, 1 ) -
00253         mat( 0, 1 ) * mat( 2, 0 ) * mat( 3, 2 ) -
00254         mat( 0, 2 ) * mat( 2, 1 ) * mat( 3, 0 );
00255 
00256     result( 3, 2 ) =
00257         mat( 0, 0 ) * mat( 1, 2 ) * mat( 3, 1 ) +
00258         mat( 0, 1 ) * mat( 1, 0 ) * mat( 3, 2 ) +
00259         mat( 0, 2 ) * mat( 1, 1 ) * mat( 3, 0 ) -
00260         mat( 0, 0 ) * mat( 1, 1 ) * mat( 3, 2 ) -
00261         mat( 0, 1 ) * mat( 1, 2 ) * mat( 3, 0 ) -
00262         mat( 0, 2 ) * mat( 1, 0 ) * mat( 3, 1 );
00263 
00264     result( 3, 3 ) =
00265         mat( 0, 0 ) * mat( 1, 1 ) * mat( 2, 2 ) +
00266         mat( 0, 1 ) * mat( 1, 2 ) * mat( 2, 0 ) +
00267         mat( 0, 2 ) * mat( 1, 0 ) * mat( 2, 1 ) -
00268         mat( 0, 0 ) * mat( 1, 2 ) * mat( 2, 1 ) -
00269         mat( 0, 1 ) * mat( 1, 0 ) * mat( 2, 2 ) -
00270         mat( 0, 2 ) * mat( 1, 1 ) * mat( 2, 0 );
00271 
00272     WAssert( det != 0, "Determinat is zero. This matrix can not be inverted." );
00273 
00274     double detInv = 1. / det;
00275     for( size_t r = 0; r < 4; ++r)
00276     {
00277         for( size_t c = 0; c < 4; ++c )
00278         {
00279             result( c, r ) *= detInv;
00280         }
00281     }
00282 
00283     return result;
00284 }
00285 
00286 bool linearIndependent( const WVector3d& u, const WVector3d& v )
00287 {
00288     WVector3d cp = cross( u, v );
00289     if( std::fabs( cp[0] ) < wlimits::DBL_EPS && std::fabs( cp[1] ) < wlimits::DBL_EPS && std::fabs( cp[2] ) < wlimits::DBL_EPS )
00290     {
00291         return false;
00292     }
00293     return true;
00294 }
00295 
00296 void computeSVD( const WMatrix<double>& A,
00297                         WMatrix<double>& U,
00298                         WMatrix<double>& V,
00299                         WValue<double>& S )
00300 {
00301     Eigen::MatrixXd eigenA( A );
00302     Eigen::JacobiSVD<Eigen::MatrixXd> svd( eigenA, Eigen::ComputeFullU | Eigen::ComputeFullV );
00303     U = WMatrix<double>( svd.matrixU() );
00304     V = WMatrix<double>( svd.matrixV() );
00305     S = WValue<double>( svd.singularValues() );
00306 }
00307 
00308 WMatrix<double> pseudoInverse( const WMatrix<double>& input )
00309 {
00310     // calc pseudo inverse
00311     WMatrix<double> U( input.getNbRows(), input.getNbCols() );
00312     WMatrix<double> V( input.getNbCols(), input.getNbCols() );
00313     WValue<double> Svec( input.size() );
00314     computeSVD( input, U, V, Svec );
00315 
00316     // create diagonal matrix S
00317     WMatrix<double> S( input.getNbCols(), input.getNbCols() );
00318     S.setZero();
00319     for( size_t i = 0; i < Svec.size() && i < S.getNbRows() && i < S.getNbCols(); i++ )
00320     {
00321         S( i, i ) = ( Svec[ i ] == 0.0 ) ? 0.0 : 1.0 / Svec[ i ];
00322     }
00323 
00324     return WMatrix<double>( V*S*U.transposed() );
00325 }
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends