00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
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] ;
00050 resultVec4D[1] = mat( 1, 0 ) * vec[0] + mat( 1, 1 ) * vec[1] + mat( 1, 2 ) * vec[2] ;
00051 resultVec4D[2] = mat( 2, 0 ) * vec[0] + mat( 2, 1 ) * vec[1] + mat( 2, 2 ) * vec[2] ;
00052 resultVec4D[3] = mat( 3, 0 ) * vec[0] + mat( 3, 1 ) * vec[1] + mat( 3, 2 ) * vec[2] ;
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
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
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 }