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