![]() |
Tulip
4.6.0
Better Visualization Through Research
|
00001 /* 00002 * 00003 * This file is part of Tulip (www.tulip-software.org) 00004 * 00005 * Authors: David Auber and the Tulip development Team 00006 * from LaBRI, University of Bordeaux 00007 * 00008 * Tulip is free software; you can redistribute it and/or modify 00009 * it under the terms of the GNU Lesser General Public License 00010 * as published by the Free Software Foundation, either version 3 00011 * of the License, or (at your option) any later version. 00012 * 00013 * Tulip is distributed in the hope that it will be useful, 00014 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00015 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 00016 * See the GNU General Public License for more details. 00017 * 00018 */ 00019 #include <cmath> 00020 00021 //=================================================================== 00022 //Specialisation 00023 namespace tlp { 00024 template<typename Obj> 00025 class Matrix<Obj,1>:public Array<Vector<Obj,1>,1> { 00026 public: 00027 Obj determinant() { 00028 return (*this)[0][0]; 00029 } 00030 // Matrix<Obj,1>& fill(Obj obj) {return *this;} 00031 Matrix<Obj,1>& inverse() { 00032 (*this)[0][0] = 1.0 / (*this)[0][0]; 00033 return *this; 00034 } 00035 Matrix<Obj,1>& transpose() { 00036 return *this; 00037 } 00038 Matrix<Obj,1>& operator*=(const Matrix<Obj,1> &mat) { 00039 (*this)[0][0] *= mat[0][0]; 00040 return *this; 00041 } 00042 // Matrix<Obj,1>& operator/=(const Obj &obj){return *this;} 00043 Matrix<Obj,1> cofactor() { 00044 return *this; 00045 } 00046 }; 00047 } 00048 00049 00050 //=================================================================== 00051 template<typename Obj,unsigned int SIZE> 00052 MATRIX::Matrix(const std::vector<std::vector<Obj> > &covarianceMatrix) { 00053 for(unsigned int i=0; i < SIZE; i++) 00054 for(unsigned int j=0; j < SIZE; j++) 00055 (*this)[i][j] = covarianceMatrix[i][j] / (sqrt(covarianceMatrix[i][i] * covarianceMatrix[j][j])); 00056 } 00057 00058 //=================================================================== 00059 template<typename Obj,unsigned int SIZE> 00060 MATRIX & MATRIX::fill(Obj obj) { 00061 for (unsigned int i=0; i<SIZE; ++i) 00062 (*this)[i].fill(obj); 00063 00064 return (*this); 00065 } 00066 //====================================================== 00067 template<typename Obj,unsigned int SIZE> 00068 MATRIX& MATRIX::operator+=(const MATRIX &mat) { 00069 for (unsigned int i=0; i<SIZE; ++i) 00070 (*this)[i] += mat[i]; 00071 00072 return (*this); 00073 } 00074 //====================================================== 00075 template<typename Obj,unsigned int SIZE> 00076 MATRIX& MATRIX::operator-=(const MATRIX &mat) { 00077 for (unsigned int i=0; i<SIZE; ++i) 00078 (*this)[i] -= mat[i]; 00079 00080 return (*this); 00081 } 00082 //====================================================== 00083 template<typename Obj,unsigned int SIZE> 00084 bool MATRIX::operator==(const MATRIX &mat) const { 00085 for (unsigned int i=0; i<SIZE; ++i) { 00086 if (((*this)[i] != mat[i])) 00087 return false; 00088 } 00089 00090 return true; 00091 } 00092 //====================================================== 00093 template<typename Obj,unsigned int SIZE> 00094 bool MATRIX::operator!=(const MATRIX &mat) const { 00095 for (unsigned int i=0; i<SIZE; ++i) { 00096 if (((*this)[i] != mat[i])) 00097 return true; 00098 } 00099 00100 return false; 00101 } 00102 //=================================================================== 00103 template<typename Obj,unsigned int SIZE> 00104 MATRIX & MATRIX::operator*=(const MATRIX &mat) { 00105 (*this) = (*this) * mat; 00106 return (*this); 00107 } 00108 //===================================================================================== 00109 template<typename Obj,unsigned int SIZE> 00110 MATRIX & MATRIX::operator*=(const Obj &obj) { 00111 for (unsigned int i=0; i<SIZE; ++i) 00112 (*this)[i] *= obj; 00113 00114 return (*this); 00115 } 00116 //===================================================================================== 00117 template<typename Obj,unsigned int SIZE> 00118 MATRIX & MATRIX::operator/=(const MATRIX &mat) { 00119 MATRIX tmpMat(mat); 00120 tmpMat.inverse(); 00121 (*this) *= tmpMat; 00122 return (*this); 00123 } 00124 //===================================================================================== 00125 template<typename Obj,unsigned int SIZE> 00126 MATRIX & MATRIX::operator/=(const Obj &obj) { 00127 for (unsigned int i=0; i<SIZE; ++i) 00128 (*this)[i] /= obj; 00129 00130 return (*this); 00131 } 00132 //===================================================================================== 00133 template<typename Obj,unsigned int SIZE> 00134 Obj MATRIX::determinant() const { 00135 switch (SIZE) { 00136 case 2: 00137 return (*this)[0][0] * (*this)[1][1] - (*this)[1][0] * (*this)[0][1]; 00138 break; 00139 00140 case 3: 00141 return (*this)[0][0] * ((*this)[1][1]*(*this)[2][2] - (*this)[1][2] * (*this)[2][1]) 00142 - (*this)[0][1] * ((*this)[1][0]*(*this)[2][2] - (*this)[1][2] * (*this)[2][0]) 00143 + (*this)[0][2] * ((*this)[1][0]*(*this)[2][1] - (*this)[1][1] * (*this)[2][0]) ; 00144 break; 00145 00146 default: 00147 int j2; 00148 Obj det = 0; 00149 00150 for (unsigned int j1=0; j1<SIZE; ++j1) { 00151 tlp::Matrix<Obj, SIZE - 1> m; 00152 00153 for (unsigned int i=1; i<SIZE; i++) { 00154 j2 = 0; 00155 00156 for (unsigned int j=0; j<SIZE; ++j) { 00157 if (j == j1) 00158 continue; 00159 00160 m[i-1][j2] = (*this)[i][j]; 00161 ++j2; 00162 } 00163 } 00164 00165 if (j1 & 1) 00166 det += (*this)[0][j1] * m.determinant(); 00167 else 00168 det -= (*this)[0][j1] * m.determinant(); 00169 } 00170 00171 return(det); 00172 } 00173 } 00174 //===================================================================================== 00175 template<typename Obj,unsigned int SIZE> 00176 MATRIX MATRIX::cofactor() const { 00177 MATRIX result; 00178 00179 switch (SIZE) { 00180 case 2: 00181 (result)[0][0] = (*this)[1][1]; 00182 (result)[0][1] = - (*this)[1][0]; 00183 (result)[1][0] = - (*this)[0][1]; 00184 (result)[1][1] = (*this)[0][0]; 00185 break; 00186 00187 case 3: 00188 (result)[0][0] = (*this)[1][1]*(*this)[2][2] - (*this)[1][2]*(*this)[2][1]; 00189 (result)[0][1] = - ((*this)[1][0]*(*this)[2][2] - (*this)[2][0]*(*this)[1][2]); 00190 (result)[0][2] = (*this)[1][0]*(*this)[2][1] - (*this)[1][1]*(*this)[2][0]; 00191 (result)[1][0] = - ((*this)[0][1]*(*this)[2][2] - (*this)[0][2]*(*this)[2][1]); 00192 (result)[1][1] = (*this)[0][0]*(*this)[2][2] - (*this)[0][2]*(*this)[2][0]; 00193 (result)[1][2] = - ((*this)[0][0]*(*this)[2][1] - (*this)[0][1]*(*this)[2][0]); 00194 (result)[2][0] = (*this)[0][1]*(*this)[1][2] - (*this)[0][2]*(*this)[1][1]; 00195 (result)[2][1] = - ((*this)[0][0]*(*this)[1][2] - (*this)[0][2]*(*this)[1][0]); 00196 (result)[2][2] = (*this)[0][0]*(*this)[1][1] - (*this)[0][1]*(*this)[1][0]; 00197 break; 00198 00199 default : 00200 int i1,j1; 00201 tlp::Matrix<Obj,SIZE - 1> c; 00202 00203 for (unsigned int j=0; j<SIZE; ++j) { 00204 for (unsigned int i=0; i<SIZE; ++i) { 00205 i1 = 0; 00206 00207 for (unsigned int ii=0; ii<SIZE; ++ii) { 00208 if (ii == i) 00209 continue; 00210 00211 j1 = 0; 00212 00213 for (unsigned int jj=0; jj<SIZE; jj++) { 00214 if (jj == j) 00215 continue; 00216 00217 c[i1][j1] = (*this)[ii][jj]; 00218 ++j1; 00219 } 00220 00221 ++i1; 00222 } 00223 00224 if ((i+j) & 1) result[i][j]=c.determinant(); 00225 else result[i][j]=-c.determinant(); 00226 } 00227 } 00228 00229 break; 00230 } 00231 00232 return result; 00233 } 00234 //===================================================================================== 00235 template<typename Obj,unsigned int SIZE> 00236 MATRIX & MATRIX::transpose() { 00237 register Obj tmp; 00238 00239 for (unsigned int i=1; i<SIZE; ++i) { 00240 for (unsigned int j=0; j<i; ++j) { 00241 tmp = (*this)[i][j]; 00242 (*this)[i][j] = (*this)[j][i]; 00243 (*this)[j][i] = tmp; 00244 } 00245 } 00246 00247 return (*this); 00248 } 00249 //===================================================================================== 00250 template<typename Obj,unsigned int SIZE> 00251 MATRIX & MATRIX::inverse() { 00252 (*this) = (*this).cofactor().transpose() /= (*this).determinant(); 00253 return (*this); 00254 } 00255 //===================================================================================== 00256 template<typename Obj,unsigned int SIZE> 00257 MATRIX tlp::operator+(const MATRIX &mat1 ,const MATRIX &mat2) { 00258 return MATRIX(mat1)+=mat2; 00259 } 00260 //===================================================================================== 00261 template<typename Obj,unsigned int SIZE> 00262 MATRIX tlp::operator-(const MATRIX &mat1 ,const MATRIX &mat2) { 00263 return MATRIX(mat1)-=mat2; 00264 } 00265 //===================================================================================== 00266 template<typename Obj,unsigned int SIZE> 00267 MATRIX tlp::operator*(const MATRIX &mat1 ,const MATRIX &mat2) { 00268 MATRIX result; 00269 00270 for (unsigned int i=0; i<SIZE; ++i) 00271 for (unsigned int j=0; j<SIZE; ++j) { 00272 result[i][j] = mat1[i][0] * mat2[0][j]; 00273 00274 for (unsigned int k=1; k<SIZE; ++k) 00275 result[i][j] += mat1[i][k] * mat2[k][j]; 00276 } 00277 00278 return result; 00279 } 00280 //===================================================================================== 00281 template<typename Obj,unsigned int SIZE> 00282 MATRIX MATRIX::operator/(const MATRIX &mat2) const { 00283 return MATRIX(*this)/=mat2; 00284 } 00285 //===================================================================================== 00286 template<typename Obj,unsigned int SIZE> 00287 MATRIX MATRIX::operator/(const Obj &obj) const { 00288 return MATRIX(*this) /= obj; 00289 } 00290 //===================================================================================== 00291 template<typename Obj,unsigned int SIZE> 00292 MATRIX tlp::operator*(const MATRIX &mat ,const Obj &obj) { 00293 return MATRIX(mat) *= obj; 00294 } 00295 //===================================================================================== 00296 template<typename Obj, unsigned int SIZE> 00297 tlp::Vector<Obj, SIZE> tlp::operator*( const MATRIX &mat , const tlp::Vector<Obj, SIZE> &vec) { 00298 tlp::Vector<Obj,SIZE> result; 00299 00300 for (unsigned int row=0; row<SIZE; ++row) { 00301 result[row] = mat[row][0] * vec[0]; 00302 } 00303 00304 for (unsigned int col=1; col<SIZE; ++col) { 00305 for (unsigned int row=0; row<SIZE; ++row) { 00306 result[row] += mat[row][col] * vec[col]; 00307 } 00308 } 00309 00310 return result; 00311 } 00312 //===================================================================================== 00313 template<typename Obj,unsigned int SIZE> 00314 tlp::Vector<Obj,SIZE> tlp::operator*( const tlp::Vector<Obj,SIZE> &vec, const MATRIX &mat) { 00315 tlp::Vector<Obj,SIZE> result; 00316 00317 for (unsigned int row=0; row<SIZE; ++row) { 00318 result[row] = mat[0][row] * vec[0]; 00319 } 00320 00321 for (unsigned int col=1; col<SIZE; ++col) { 00322 for (unsigned int row=0; row<SIZE; ++row) { 00323 result[row] += mat[col][row] * vec[col]; 00324 } 00325 } 00326 00327 return result; 00328 } 00329 00330 //===================================================================================== 00331 template<typename Obj, unsigned int SIZE> 00332 tlp::Vector<Obj, SIZE> MATRIX::powerIteration(const unsigned int nIterations) const { 00333 tlp::Vector<Obj, SIZE> iteration; 00334 00335 for(unsigned int i=0; i < SIZE; i++) 00336 iteration[i] = 1; 00337 00338 for(unsigned int i=0; i < nIterations; i++) { 00339 iteration = (*this) * iteration; 00340 00341 iteration /= iteration.norm(); 00342 } 00343 00344 return iteration; 00345 } 00346 00347 //===================================================================================== 00348 00349 template<typename Obj, unsigned int SIZE> 00350 bool MATRIX::simplify(tlp::Matrix<Obj, 2> &simplifiedMatrix) const { 00351 if (SIZE != 3) { 00352 tlp::warning() << "Computation allowed only for 3x3 Matrices. Yours sizes : " << SIZE << "x" << SIZE << std::endl; 00353 00354 return false; 00355 } 00356 00357 // We start with a matrix representing an equation system under the following form : 00358 // 00359 // ax + by + cz = 0 00360 // dx + ey + fz = 0 00361 // gx + hy + iz = 0 00362 // 00363 // So M looks like : 00364 // 00365 // ( ax by cz ) *(e1)* 00366 // M = ( dx ey fz ) *(e2)* 00367 // ( gx hy iz ) *(e3)* 00368 // 00369 // What we want is something like that : 00370 // 00371 // jx + ky = 0 00372 // lx + mz = 0 00373 // 00374 // So to reduce the matrix, we will use the Gaussian Elimination. 00375 // For the first line we will apply a Gaussian Elimination between (e1) and (e2) 00376 // For the second line we will apply a Gaussian Elimination between (e1) and (e3) 00377 00378 float coeff; 00379 00380 // First Gaussian Elimination : 00381 // The pivot is z 00382 00383 coeff = (*this)[1][2] / (*this)[0][2]; // fz / cz 00384 00385 // After that: 00386 // jx = dx - (coeff * ax) 00387 // ky = ey - (coeff * by) 00388 simplifiedMatrix[0][0] = (*this)[1][0] - (coeff * (*this)[0][0]); 00389 simplifiedMatrix[0][1] = (*this)[1][1] - (coeff * (*this)[0][1]); 00390 00391 // Second Gaussian Elimination : 00392 // The pivot is y 00393 00394 coeff = (*this)[2][1] / (*this)[0][1]; // hy / by 00395 00396 // Idem : 00397 // lx = gx - (coeff * ax) 00398 // mz = iz - (coeff * cz) 00399 simplifiedMatrix[1][0] = (*this)[2][0] - (coeff * (*this)[0][0]); 00400 simplifiedMatrix[1][1] = (*this)[2][2] - (coeff * (*this)[0][2]); 00401 00402 return true; 00403 } 00404 00405 //===================================================================================== 00406 00407 template<typename Obj, unsigned int SIZE> 00408 bool MATRIX::computeEigenVector(const float x, tlp::Vector<Obj, 3> &eigenVector) const { 00409 if (SIZE != 2) { 00410 tlp::warning() << "Computation allowed only for 2x2 Matrices. Yours sizes : " << SIZE << "x" << SIZE << std::endl; 00411 00412 return false; 00413 } 00414 00415 eigenVector[0] = x; // Fixed by user 00416 00417 // We know that the matrix we are using is under that form : 00418 // 00419 // ( ax by ) 00420 // M = ( ) 00421 // ( cx dz ) 00422 // 00423 // Since we have a fixed x, we can compute y and z : 00424 // 00425 // y = -a / b 00426 // z = -c / d 00427 00428 float a, b, c, d; 00429 00430 a = (*this)[0][0]; 00431 b = (*this)[0][1]; 00432 c = (*this)[1][0]; 00433 d = (*this)[1][1]; 00434 00435 eigenVector[1] = (-a * x) / b; 00436 eigenVector[2] = (-c * x) / d; 00437 00438 return true; 00439 }