OpenWalnut  1.4.0
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 "../WAssert.h"
00028 #include "../WLimits.h"
00029 
00030 #include "WLinearAlgebraFunctions.h"
00031 #include "WMatrix.h"
00032 #include "linearAlgebra/WVectorFixed.h"
00033 
00034 WVector3d multMatrixWithVector3D( WMatrix<double> mat, WVector3d vec )
00035 {
00036     WVector3d result;
00037     result[0] = mat( 0, 0 ) * vec[0] + mat( 0, 1 ) * vec[1] + mat( 0, 2 ) * vec[2];
00038     result[1] = mat( 1, 0 ) * vec[0] + mat( 1, 1 ) * vec[1] + mat( 1, 2 ) * vec[2];
00039     result[2] = mat( 2, 0 ) * vec[0] + mat( 2, 1 ) * vec[1] + mat( 2, 2 ) * vec[2];
00040     return result;
00041 }
00042 
00043 WVector3d transformVector3DWithMatrix4D( WMatrix<double> mat, WVector3d vec )
00044 {
00045     WAssert( mat.getNbRows() == 4 && mat.getNbCols() == 4, "Matrix has wrong size." );
00046     std::vector< double > resultVec4D( 4 );
00047     resultVec4D[0] = mat( 0, 0 ) * vec[0] + mat( 0, 1 ) * vec[1] + mat( 0, 2 ) * vec[2] /* + mat( 0, 3 ) * 0 */;
00048     resultVec4D[1] = mat( 1, 0 ) * vec[0] + mat( 1, 1 ) * vec[1] + mat( 1, 2 ) * vec[2] /* + mat( 1, 3 ) * 0 */;
00049     resultVec4D[2] = mat( 2, 0 ) * vec[0] + mat( 2, 1 ) * vec[1] + mat( 2, 2 ) * vec[2] /* + mat( 2, 3 ) * 0 */;
00050     resultVec4D[3] = mat( 3, 0 ) * vec[0] + mat( 3, 1 ) * vec[1] + mat( 3, 2 ) * vec[2] /* + mat( 3, 3 ) * 0 */;
00051 
00052     WVector3d result;
00053     result[0] = resultVec4D[0] / resultVec4D[3];
00054     result[1] = resultVec4D[1] / resultVec4D[3];
00055     result[2] = resultVec4D[2] / resultVec4D[3];
00056     return result;
00057 }
00058 
00059 WPosition transformPosition3DWithMatrix4D( WMatrix<double> mat, WPosition vec )
00060 {
00061     WAssert( mat.getNbRows() == 4 && mat.getNbCols() == 4, "Matrix has wrong size." );
00062     std::vector< double > resultVec4D( 4 );
00063     resultVec4D[0] = mat( 0, 0 ) * vec[0] + mat( 0, 1 ) * vec[1] + mat( 0, 2 ) * vec[2] + mat( 0, 3 ) * 1;
00064     resultVec4D[1] = mat( 1, 0 ) * vec[0] + mat( 1, 1 ) * vec[1] + mat( 1, 2 ) * vec[2] + mat( 1, 3 ) * 1;
00065     resultVec4D[2] = mat( 2, 0 ) * vec[0] + mat( 2, 1 ) * vec[1] + mat( 2, 2 ) * vec[2] + mat( 2, 3 ) * 1;
00066     resultVec4D[3] = mat( 3, 0 ) * vec[0] + mat( 3, 1 ) * vec[1] + mat( 3, 2 ) * vec[2] + mat( 3, 3 ) * 1;
00067 
00068     WPosition result;
00069     result[0] = resultVec4D[0] / resultVec4D[3];
00070     result[1] = resultVec4D[1] / resultVec4D[3];
00071     result[2] = resultVec4D[2] / resultVec4D[3];
00072     return result;
00073 }
00074 
00075 WMatrix<double> invertMatrix3x3( WMatrix<double> mat )
00076 {
00077     WAssert( mat.getNbRows(), "Zero rows found." );
00078     WAssert( mat.getNbCols(), "Zero columns found." );
00079     double det = mat( 0, 0 ) * mat( 1, 1 ) * mat( 2, 2 ) +
00080                 mat( 0, 1 ) * mat( 1, 2 ) * mat( 2, 0 ) +
00081                 mat( 0, 2 ) * mat( 1, 0 ) * mat( 2, 1 ) -
00082                 mat( 0, 2 ) * mat( 1, 1 ) * mat( 2, 0 ) -
00083                 mat( 0, 1 ) * mat( 1, 0 ) * mat( 2, 2 ) -
00084                 mat( 0, 0 ) * mat( 1, 2 ) * mat( 2, 1 );
00085 
00086     WAssert( det != 0, "Determinant is zero. This matrix can not be inverted." );
00087 
00088     WMatrix<double> r( 3, 3 );
00089 
00090     r( 0, 0 ) = ( mat( 1, 1 ) * mat( 2, 2 ) - mat(  1, 2 ) * mat( 2, 1 ) ) / det;
00091     r( 1, 0 ) = ( mat( 1, 2 ) * mat( 2, 0 ) - mat(  1, 0 ) * mat( 2, 2 ) ) / det;
00092     r( 2, 0 ) = ( mat( 1, 0 ) * mat( 2, 1 ) - mat(  1, 1 ) * mat( 2, 0 ) ) / det;
00093     r( 0, 1 ) = ( mat( 0, 2 ) * mat( 2, 1 ) - mat(  0, 1 ) * mat( 2, 2 ) ) / det;
00094     r( 1, 1 ) = ( mat( 0, 0 ) * mat( 2, 2 ) - mat(  0, 2 ) * mat( 2, 0 ) ) / det;
00095     r( 2, 1 ) = ( mat( 0, 1 ) * mat( 2, 0 ) - mat(  0, 0 ) * mat( 2, 1 ) ) / det;
00096     r( 0, 2 ) = ( mat( 0, 1 ) * mat( 1, 2 ) - mat(  0, 2 ) * mat( 1, 1 ) ) / det;
00097     r( 1, 2 ) = ( mat( 0, 2 ) * mat( 1, 0 ) - mat(  0, 0 ) * mat( 1, 2 ) ) / det;
00098     r( 2, 2 ) = ( mat( 0, 0 ) * mat( 1, 1 ) - mat(  0, 1 ) * mat( 1, 0 ) ) / det;
00099 
00100     return r;
00101 }
00102 
00103 WMatrix<double> invertMatrix4x4( WMatrix<double> mat )
00104 {
00105     WAssert( mat.getNbRows(), "Zero rows found." );
00106     WAssert( mat.getNbCols(), "Zero columns found." );
00107     double det =
00108         mat( 0, 0 ) * mat( 1, 1 ) * mat( 2, 2 ) * mat( 3, 3 ) +
00109         mat( 0, 0 ) * mat( 1, 2 ) * mat( 2, 3 ) * mat( 3, 1 ) +
00110         mat( 0, 0 ) * mat( 1, 3 ) * mat( 2, 1 ) * mat( 3, 2 ) +
00111 
00112         mat( 0, 1 ) * mat( 1, 0 ) * mat( 2, 3 ) * mat( 3, 2 ) +
00113         mat( 0, 1 ) * mat( 1, 2 ) * mat( 2, 0 ) * mat( 3, 3 ) +
00114         mat( 0, 1 ) * mat( 1, 3 ) * mat( 2, 2 ) * mat( 3, 0 ) +
00115 
00116         mat( 0, 2 ) * mat( 1, 0 ) * mat( 2, 1 ) * mat( 3, 3 ) +
00117         mat( 0, 2 ) * mat( 1, 1 ) * mat( 2, 3 ) * mat( 3, 0 ) +
00118         mat( 0, 2 ) * mat( 1, 3 ) * mat( 2, 0 ) * mat( 3, 1 ) +
00119 
00120         mat( 0, 3 ) * mat( 1, 0 ) * mat( 2, 2 ) * mat( 3, 1 ) +
00121         mat( 0, 3 ) * mat( 1, 1 ) * mat( 2, 0 ) * mat( 3, 2 ) +
00122         mat( 0, 3 ) * mat( 1, 2 ) * mat( 2, 1 ) * mat( 3, 0 ) -
00123 
00124         mat( 0, 0 ) * mat( 1, 1 ) * mat( 2, 3 ) * mat( 3, 2 ) -
00125         mat( 0, 0 ) * mat( 1, 2 ) * mat( 2, 1 ) * mat( 3, 3 ) -
00126         mat( 0, 0 ) * mat( 1, 3 ) * mat( 2, 2 ) * mat( 3, 1 ) -
00127 
00128         mat( 0, 1 ) * mat( 1, 0 ) * mat( 2, 2 ) * mat( 3, 3 ) -
00129         mat( 0, 1 ) * mat( 1, 2 ) * mat( 2, 3 ) * mat( 3, 0 ) -
00130         mat( 0, 1 ) * mat( 1, 3 ) * mat( 2, 0 ) * mat( 3, 2 ) -
00131 
00132         mat( 0, 2 ) * mat( 1, 0 ) * mat( 2, 3 ) * mat( 3, 1 ) -
00133         mat( 0, 2 ) * mat( 1, 1 ) * mat( 2, 0 ) * mat( 3, 3 ) -
00134         mat( 0, 2 ) * mat( 1, 3 ) * mat( 2, 1 ) * mat( 3, 0 ) -
00135 
00136         mat( 0, 3 ) * mat( 1, 0 ) * mat( 2, 1 ) * mat( 3, 2 ) -
00137         mat( 0, 3 ) * mat( 1, 1 ) * mat( 2, 2 ) * mat( 3, 0 ) -
00138         mat( 0, 3 ) * mat( 1, 2 ) * mat( 2, 0 ) * mat( 3, 1 );
00139 
00140     WMatrix<double> result( 4, 4 );
00141 
00142     result( 0, 0 ) =
00143         mat( 1, 1 ) * mat( 2, 2 ) * mat( 3, 3 ) +
00144         mat( 1, 2 ) * mat( 2, 3 ) * mat( 3, 1 ) +
00145         mat( 1, 3 ) * mat( 2, 1 ) * mat( 3, 2 ) -
00146         mat( 1, 1 ) * mat( 2, 3 ) * mat( 3, 2 ) -
00147         mat( 1, 2 ) * mat( 2, 1 ) * mat( 3, 3 ) -
00148         mat( 1, 3 ) * mat( 2, 2 ) * mat( 3, 1 );
00149 
00150     result( 0, 1 ) =
00151         mat( 0, 1 ) * mat( 2, 3 ) * mat( 3, 2 ) +
00152         mat( 0, 2 ) * mat( 2, 1 ) * mat( 3, 3 ) +
00153         mat( 0, 3 ) * mat( 2, 2 ) * mat( 3, 1 ) -
00154         mat( 0, 1 ) * mat( 2, 2 ) * mat( 3, 3 ) -
00155         mat( 0, 2 ) * mat( 2, 3 ) * mat( 3, 1 ) -
00156         mat( 0, 3 ) * mat( 2, 1 ) * mat( 3, 2 );
00157 
00158     result( 0, 2 ) =
00159         mat( 0, 1 ) * mat( 1, 2 ) * mat( 3, 3 ) +
00160         mat( 0, 2 ) * mat( 1, 3 ) * mat( 3, 1 ) +
00161         mat( 0, 3 ) * mat( 1, 1 ) * mat( 3, 2 ) -
00162         mat( 0, 1 ) * mat( 1, 3 ) * mat( 3, 2 ) -
00163         mat( 0, 2 ) * mat( 1, 1 ) * mat( 3, 3 ) -
00164         mat( 0, 3 ) * mat( 1, 2 ) * mat( 3, 1 );
00165 
00166     result( 0, 3 ) =
00167         mat( 0, 1 ) * mat( 1, 3 ) * mat( 2, 2 ) +
00168         mat( 0, 2 ) * mat( 1, 1 ) * mat( 2, 3 ) +
00169         mat( 0, 3 ) * mat( 1, 2 ) * mat( 2, 1 ) -
00170         mat( 0, 1 ) * mat( 1, 2 ) * mat( 2, 3 ) -
00171         mat( 0, 2 ) * mat( 1, 3 ) * mat( 2, 1 ) -
00172         mat( 0, 3 ) * mat( 1, 1 ) * mat( 2, 2 );
00173 
00174     result( 1, 0 ) =
00175         mat( 1, 0 ) * mat( 2, 3 ) * mat( 3, 2 ) +
00176         mat( 1, 2 ) * mat( 2, 0 ) * mat( 3, 3 ) +
00177         mat( 1, 3 ) * mat( 2, 2 ) * mat( 3, 0 ) -
00178         mat( 1, 0 ) * mat( 2, 2 ) * mat( 3, 3 ) -
00179         mat( 1, 2 ) * mat( 2, 3 ) * mat( 3, 0 ) -
00180         mat( 1, 3 ) * mat( 2, 0 ) * mat( 3, 2 );
00181 
00182     result( 1, 1 ) =
00183         mat( 0, 0 ) * mat( 2, 2 ) * mat( 3, 3 ) +
00184         mat( 0, 2 ) * mat( 2, 3 ) * mat( 3, 0 ) +
00185         mat( 0, 3 ) * mat( 2, 0 ) * mat( 3, 2 ) -
00186         mat( 0, 0 ) * mat( 2, 3 ) * mat( 3, 2 ) -
00187         mat( 0, 2 ) * mat( 2, 0 ) * mat( 3, 3 ) -
00188         mat( 0, 3 ) * mat( 2, 2 ) * mat( 3, 0 );
00189 
00190     result( 1, 2 ) =
00191         mat( 0, 0 ) * mat( 1, 3 ) * mat( 3, 2 ) +
00192         mat( 0, 2 ) * mat( 1, 0 ) * mat( 3, 3 ) +
00193         mat( 0, 3 ) * mat( 1, 2 ) * mat( 3, 0 ) -
00194         mat( 0, 0 ) * mat( 1, 2 ) * mat( 3, 3 ) -
00195         mat( 0, 2 ) * mat( 1, 3 ) * mat( 3, 0 ) -
00196         mat( 0, 3 ) * mat( 1, 0 ) * mat( 3, 2 );
00197 
00198     result( 1, 3 ) =
00199         mat( 0, 0 ) * mat( 1, 2 ) * mat( 2, 3 ) +
00200         mat( 0, 2 ) * mat( 1, 3 ) * mat( 2, 0 ) +
00201         mat( 0, 3 ) * mat( 1, 0 ) * mat( 2, 2 ) -
00202         mat( 0, 0 ) * mat( 1, 3 ) * mat( 2, 2 ) -
00203         mat( 0, 2 ) * mat( 1, 0 ) * mat( 2, 3 ) -
00204         mat( 0, 3 ) * mat( 1, 2 ) * mat( 2, 0 );
00205 
00206     result( 2, 0 ) =
00207         mat( 1, 0 ) * mat( 2, 1 ) * mat( 3, 3 ) +
00208         mat( 1, 1 ) * mat( 2, 3 ) * mat( 3, 0 ) +
00209         mat( 1, 3 ) * mat( 2, 0 ) * mat( 3, 1 ) -
00210         mat( 1, 0 ) * mat( 2, 3 ) * mat( 3, 1 ) -
00211         mat( 1, 1 ) * mat( 2, 0 ) * mat( 3, 3 ) -
00212         mat( 1, 3 ) * mat( 2, 1 ) * mat( 3, 0 );
00213 
00214     result( 2, 1 ) =
00215         mat( 0, 0 ) * mat( 2, 3 ) * mat( 3, 1 ) +
00216         mat( 0, 1 ) * mat( 2, 0 ) * mat( 3, 3 ) +
00217         mat( 0, 3 ) * mat( 2, 1 ) * mat( 3, 0 ) -
00218         mat( 0, 0 ) * mat( 2, 1 ) * mat( 3, 3 ) -
00219         mat( 0, 1 ) * mat( 2, 3 ) * mat( 3, 0 ) -
00220         mat( 0, 3 ) * mat( 2, 0 ) * mat( 3, 1 );
00221 
00222     result( 2, 2 ) =
00223         mat( 0, 0 ) * mat( 1, 1 ) * mat( 3, 3 ) +
00224         mat( 0, 1 ) * mat( 1, 3 ) * mat( 3, 0 ) +
00225         mat( 0, 3 ) * mat( 1, 0 ) * mat( 3, 1 ) -
00226         mat( 0, 0 ) * mat( 1, 3 ) * mat( 3, 1 ) -
00227         mat( 0, 1 ) * mat( 1, 0 ) * mat( 3, 3 ) -
00228         mat( 0, 3 ) * mat( 1, 1 ) * mat( 3, 0 );
00229 
00230     result( 2, 3 ) =
00231         mat( 0, 0 ) * mat( 1, 3 ) * mat( 2, 1 ) +
00232         mat( 0, 1 ) * mat( 1, 0 ) * mat( 2, 3 ) +
00233         mat( 0, 3 ) * mat( 1, 1 ) * mat( 2, 0 ) -
00234         mat( 0, 0 ) * mat( 1, 1 ) * mat( 2, 3 ) -
00235         mat( 0, 1 ) * mat( 1, 3 ) * mat( 2, 0 ) -
00236         mat( 0, 3 ) * mat( 1, 0 ) * mat( 2, 1 );
00237 
00238     result( 3, 0 ) =
00239         mat( 1, 0 ) * mat( 2, 2 ) * mat( 3, 1 ) +
00240         mat( 1, 1 ) * mat( 2, 0 ) * mat( 3, 2 ) +
00241         mat( 1, 2 ) * mat( 2, 1 ) * mat( 3, 0 ) -
00242         mat( 1, 0 ) * mat( 2, 1 ) * mat( 3, 2 ) -
00243         mat( 1, 1 ) * mat( 2, 2 ) * mat( 3, 0 ) -
00244         mat( 1, 2 ) * mat( 2, 0 ) * mat( 3, 1 );
00245 
00246     result( 3, 1 ) =
00247         mat( 0, 0 ) * mat( 2, 1 ) * mat( 3, 2 ) +
00248         mat( 0, 1 ) * mat( 2, 2 ) * mat( 3, 0 ) +
00249         mat( 0, 2 ) * mat( 2, 0 ) * mat( 3, 1 ) -
00250         mat( 0, 0 ) * mat( 2, 2 ) * mat( 3, 1 ) -
00251         mat( 0, 1 ) * mat( 2, 0 ) * mat( 3, 2 ) -
00252         mat( 0, 2 ) * mat( 2, 1 ) * mat( 3, 0 );
00253 
00254     result( 3, 2 ) =
00255         mat( 0, 0 ) * mat( 1, 2 ) * mat( 3, 1 ) +
00256         mat( 0, 1 ) * mat( 1, 0 ) * mat( 3, 2 ) +
00257         mat( 0, 2 ) * mat( 1, 1 ) * mat( 3, 0 ) -
00258         mat( 0, 0 ) * mat( 1, 1 ) * mat( 3, 2 ) -
00259         mat( 0, 1 ) * mat( 1, 2 ) * mat( 3, 0 ) -
00260         mat( 0, 2 ) * mat( 1, 0 ) * mat( 3, 1 );
00261 
00262     result( 3, 3 ) =
00263         mat( 0, 0 ) * mat( 1, 1 ) * mat( 2, 2 ) +
00264         mat( 0, 1 ) * mat( 1, 2 ) * mat( 2, 0 ) +
00265         mat( 0, 2 ) * mat( 1, 0 ) * mat( 2, 1 ) -
00266         mat( 0, 0 ) * mat( 1, 2 ) * mat( 2, 1 ) -
00267         mat( 0, 1 ) * mat( 1, 0 ) * mat( 2, 2 ) -
00268         mat( 0, 2 ) * mat( 1, 1 ) * mat( 2, 0 );
00269 
00270     WAssert( det != 0, "Determinat is zero. This matrix can not be inverted." );
00271 
00272     double detInv = 1. / det;
00273     for( size_t r = 0; r < 4; ++r)
00274     {
00275         for( size_t c = 0; c < 4; ++c )
00276         {
00277             result( c, r ) *= detInv;
00278         }
00279     }
00280 
00281     return result;
00282 }
00283 
00284 bool linearIndependent( const WVector3d& u, const WVector3d& v )
00285 {
00286     WVector3d cp = cross( u, v );
00287     if( std::fabs( cp[0] ) < wlimits::DBL_EPS && std::fabs( cp[1] ) < wlimits::DBL_EPS && std::fabs( cp[2] ) < wlimits::DBL_EPS )
00288     {
00289         return false;
00290     }
00291     return true;
00292 }