OpenWalnut 1.2.5
|
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 }