MOAB: Mesh Oriented datABase
(version 5.2.1)
|
00001 /* ***************************************************************** 00002 MESQUITE -- The Mesh Quality Improvement Toolkit 00003 00004 Copyright 2006 Sandia National Laboratories. Developed at the 00005 University of Wisconsin--Madison under SNL contract number 00006 624796. The U.S. Government and the University of Wisconsin 00007 retain certain rights to this software. 00008 00009 This library is free software; you can redistribute it and/or 00010 modify it under the terms of the GNU Lesser General Public 00011 License as published by the Free Software Foundation; either 00012 version 2.1 of the License, or (at your option) any later version. 00013 00014 This library is distributed in the hope that it will be useful, 00015 but WITHOUT ANY WARRANTY; without even the implied warranty of 00016 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 00017 Lesser General Public License for more details. 00018 00019 You should have received a copy of the GNU Lesser General Public License 00020 (lgpl.txt) along with this library; if not, write to the Free Software 00021 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 00022 00023 (2006) kraftche@cae.wisc.edu 00024 00025 ***************************************************************** */ 00026 00027 /** \file MsqMatrix.hpp 00028 * \brief 00029 * \author Jason Kraftcheck 00030 */ 00031 00032 #ifndef MSQ_MSQ_MATRIX_HPP 00033 #define MSQ_MSQ_MATRIX_HPP 00034 00035 #include "Mesquite.hpp" 00036 00037 #include <string> 00038 #include <istream> 00039 #include <ostream> 00040 #include <sstream> 00041 00042 namespace MBMesquite 00043 { 00044 00045 /**\brief Fixed-size matrix class 00046 * 00047 * This class implements a fixed-size 2-dimensional matrix. 00048 * The actual size is specified with template parameters. 00049 */ 00050 template < unsigned R, unsigned C > 00051 class MsqMatrix 00052 { 00053 protected: 00054 double m[R * C]; 00055 00056 public: 00057 typedef MsqMatrix< R, C > my_type; 00058 00059 enum 00060 { 00061 ROWS = R, 00062 COLS = C 00063 }; 00064 00065 /** Constructor for uninitialized matrix */ 00066 MsqMatrix() {} 00067 /** Initialize diagonal values, zero others */ 00068 MsqMatrix( double v ) 00069 { 00070 diag( v ); 00071 } 00072 /** Initialize to an array of values */ 00073 MsqMatrix( const double* v ) 00074 { 00075 set( v ); 00076 } 00077 /** Initialize from 2D array */ 00078 MsqMatrix( const double v[R][C] ) 00079 { 00080 set( v ); 00081 } 00082 /** Initialize with column vectors */ 00083 MsqMatrix( const MsqMatrix< R, 1 >* c ) 00084 { 00085 set_columns( c ); 00086 } 00087 /** Initialize with row vectors */ 00088 MsqMatrix( const MsqMatrix< 1, C >* r ) 00089 { 00090 set_rows( r ); 00091 } 00092 /** Parse initial values from string */ 00093 MsqMatrix( const char* s ) 00094 { 00095 set( s ); 00096 } 00097 /** Parse initial values from string */ 00098 MsqMatrix( const std::string& s ) 00099 { 00100 set( s ); 00101 } 00102 /** Initialize to the minor of a larger matrix 00103 * This matrix is the passed matrix with the 00104 * specified row and column removed. 00105 */ 00106 MsqMatrix( const MsqMatrix< R + 1, C + 1 >& p_m, unsigned p_r, unsigned p_c ) 00107 { 00108 make_minor( p_m, p_r, p_c ); 00109 } 00110 00111 MsqMatrix< R, C >& operator=( double v ) 00112 { 00113 set( v ); 00114 return *this; 00115 } 00116 MsqMatrix< R, C >& operator=( const double* v ) 00117 { 00118 set( v ); 00119 return *this; 00120 } 00121 MsqMatrix< R, C >& operator=( const char* s ) 00122 { 00123 set( s ); 00124 return *this; 00125 } 00126 MsqMatrix< R, C >& operator=( const std::string& s ) 00127 { 00128 set( s ); 00129 return *this; 00130 } 00131 00132 double& operator()( unsigned r, unsigned c ) 00133 { 00134 return m[r * C + c]; 00135 } 00136 double operator()( unsigned r, unsigned c ) const 00137 { 00138 return m[r * C + c]; 00139 } 00140 double* data() 00141 { 00142 return m; 00143 } 00144 const double* data() const 00145 { 00146 return m; 00147 } 00148 00149 void zero() 00150 { 00151 set( 0.0 ); 00152 } 00153 void identity() 00154 { 00155 diag( 1.0 ); 00156 } 00157 void set( double v ) 00158 { 00159 for( unsigned i = 0; i < R * C; ++i ) 00160 m[i] = v; 00161 } 00162 void set( const double* v ) 00163 { 00164 for( unsigned i = 0; i < R * C; ++i ) 00165 m[i] = v[i]; 00166 } 00167 void set( const double v[R][C] ); 00168 void set( const char* s ) 00169 { 00170 std::istringstream i( s ); 00171 i >> *this; 00172 } 00173 void set( const std::string& s ) 00174 { 00175 set( s.c_str() ); 00176 } 00177 /** Set diagonal value to passed values, others to zero. */ 00178 inline void diag( double v ); 00179 /** Set diagonal values to passed values, others to zero. */ 00180 inline void diag( const double* v ); 00181 /** Set this matrix to the minor of a larger matrix */ 00182 inline void make_minor( const MsqMatrix< R + 1, C + 1 >& m, unsigned r, unsigned c ); 00183 00184 /** *this += transpose(other) */ 00185 inline MsqMatrix< R, C >& assign_add_transpose( const MsqMatrix< C, R >& other ); 00186 /** *this = s*m */ 00187 inline MsqMatrix< R, C >& assign_product( double s, const MsqMatrix< R, C >& m ); 00188 /** *this += s*m */ 00189 inline MsqMatrix< R, C >& assign_add_product( double s, const MsqMatrix< R, C >& m ); 00190 /** multiply each element by the cooresponding element in m */ 00191 inline MsqMatrix< R, C >& assign_multiply_elements( const MsqMatrix< R, C >& m ); 00192 00193 inline MsqMatrix< R, C >& operator+=( const MsqMatrix< R, C >& other ); 00194 inline MsqMatrix< R, C >& operator-=( const MsqMatrix< R, C >& other ); 00195 inline MsqMatrix< R, C >& operator+=( double scalar ); 00196 inline MsqMatrix< R, C >& operator-=( double scalar ); 00197 inline MsqMatrix< R, C >& operator*=( double scalar ); 00198 inline MsqMatrix< R, C >& operator/=( double scalar ); 00199 00200 // MsqMatrix<1,C>& row( unsigned r ) { return *(MsqMatrix<1,C>*)(m+C*r); } 00201 // const MsqMatrix<1,C>& row( unsigned r ) const { return *(MsqMatrix<1,C>*)(m+C*r); } 00202 MsqMatrix< 1, C > row( unsigned r ) const 00203 { 00204 return MsqMatrix< 1, C >( m + C * r ); 00205 } 00206 MsqMatrix< R, 1 > column( unsigned c ) const; 00207 MsqMatrix< 1, R > column_transpose( unsigned c ) const; 00208 00209 void set_row( unsigned r, const MsqMatrix< 1, C >& v ); 00210 void add_row( unsigned r, const MsqMatrix< 1, C >& v ); 00211 void set_row_transpose( unsigned r, const MsqMatrix< C, 1 >& v ); 00212 void set_rows( const MsqMatrix< 1, C >* v ); 00213 void set_column( unsigned c, const MsqMatrix< R, 1 >& v ); 00214 void add_column( unsigned c, const MsqMatrix< R, 1 >& v ); 00215 void set_column_transpose( unsigned c, const MsqMatrix< 1, R >& v ); 00216 void set_columns( const MsqMatrix< R, 1 >* v ); 00217 }; 00218 00219 template <> 00220 class MsqMatrix< 1, 1 > 00221 { 00222 protected: 00223 double m; 00224 00225 public: 00226 typedef MsqMatrix< 1, 1 > my_type; 00227 00228 enum 00229 { 00230 ROWS = 1, 00231 COLS = 1 00232 }; 00233 00234 /** Constructor for uninitialized matrix */ 00235 MsqMatrix() {} 00236 /** Initialize diagonal values, zero others */ 00237 MsqMatrix( double v ) : m( v ) {} 00238 /** Initialize to an array of values */ 00239 MsqMatrix( const double* v ) : m( *v ) {} 00240 /** Parse initial values from string */ 00241 MsqMatrix( const char* s ) 00242 { 00243 set( s ); 00244 } 00245 /** Parse initial values from string */ 00246 MsqMatrix( const std::string& s ) 00247 { 00248 set( s ); 00249 } 00250 /** Initialize to the minor of a larger matrix 00251 * This matrix is the passed matrix with the 00252 * specified row and column removed. 00253 */ 00254 MsqMatrix( const MsqMatrix< 2, 2 >& M, unsigned r, unsigned c ) : m( M( r, c ) ) {} 00255 00256 MsqMatrix< 1, 1 >& operator=( double v ) 00257 { 00258 m = v; 00259 return *this; 00260 } 00261 MsqMatrix< 1, 1 >& operator=( const double* v ) 00262 { 00263 m = *v; 00264 return *this; 00265 } 00266 MsqMatrix< 1, 1 >& operator=( const char* s ) 00267 { 00268 set( s ); 00269 return *this; 00270 } 00271 MsqMatrix< 1, 1 >& operator=( const std::string& s ) 00272 { 00273 set( s ); 00274 return *this; 00275 } 00276 00277 double& operator()( unsigned, unsigned ) 00278 { 00279 return m; 00280 } 00281 double operator()( unsigned, unsigned ) const 00282 { 00283 return m; 00284 } 00285 double* data() 00286 { 00287 return &m; 00288 } 00289 const double* data() const 00290 { 00291 return &m; 00292 } 00293 00294 void zero() 00295 { 00296 m = 0.0; 00297 } 00298 void identity() 00299 { 00300 m = 1.0; 00301 } 00302 void set( double v ) 00303 { 00304 m = v; 00305 } 00306 void set( const double* v ) 00307 { 00308 m = *v; 00309 } 00310 void set( const char* s ) 00311 { 00312 std::istringstream i( s ); 00313 i >> m; 00314 } 00315 void set( const std::string& s ) 00316 { 00317 set( s.c_str() ); 00318 } 00319 /** Set diagonal value to passed values, others to zero. */ 00320 inline void diag( double v ) 00321 { 00322 m = v; 00323 } 00324 /** Set diagonal values to passed values, others to zero. */ 00325 inline void diag( const double* v ) 00326 { 00327 m = *v; 00328 } 00329 /** Set this matrix to the minor of a larger matrix */ 00330 inline void make_minor( const MsqMatrix< 2, 2 >& M, unsigned r, unsigned c ) 00331 { 00332 m = M( r, c ); 00333 } 00334 00335 /** *this += transpose(other) */ 00336 inline MsqMatrix< 1, 1 >& assign_add_transpose( const MsqMatrix< 1, 1 >& other ) 00337 { 00338 m += other.m; 00339 return *this; 00340 } 00341 /** *this = s*m */ 00342 inline MsqMatrix< 1, 1 >& assign_product( double s, const MsqMatrix< 1, 1 >& other ) 00343 { 00344 m = s * other.m; 00345 return *this; 00346 } 00347 /** *this += s*m */ 00348 inline MsqMatrix< 1, 1 >& assign_add_product( double s, const MsqMatrix< 1, 1 >& other ) 00349 { 00350 m += s * other.m; 00351 return *this; 00352 } 00353 /** multiply each element by the cooresponding element in m */ 00354 inline MsqMatrix< 1, 1 >& assign_multiply_elements( const MsqMatrix< 1, 1 >& other ) 00355 { 00356 m *= other.m; 00357 return *this; 00358 } 00359 00360 operator double() const 00361 { 00362 return m; 00363 } 00364 }; 00365 00366 /** \brief Vector is a 1xL Matrix 00367 * 00368 * Define a Vector as a 1xL Matrix 00369 * Add single-index access operators 00370 */ 00371 template < unsigned L > 00372 class MsqVector : public MsqMatrix< L, 1 > 00373 { 00374 public: 00375 MsqVector() {} 00376 MsqVector( double v ) 00377 { 00378 MsqMatrix< L, 1 >::set( v ); 00379 } 00380 MsqVector( const double* v ) 00381 { 00382 MsqMatrix< L, 1 >::set( v ); 00383 } 00384 MsqVector( const char* s ) 00385 { 00386 MsqMatrix< L, 1 >::set( s ); 00387 } 00388 MsqVector( const std::string& s ) 00389 { 00390 MsqMatrix< L, 1 >::set( s ); 00391 } 00392 MsqVector( const MsqMatrix< L, 1 >& pm ) : MsqMatrix< L, 1 >( pm ) {} 00393 00394 double& operator[]( unsigned idx ) 00395 { 00396 return MsqMatrix< L, 1 >::operator()( idx, 0 ); 00397 } 00398 double operator[]( unsigned idx ) const 00399 { 00400 return MsqMatrix< L, 1 >::operator()( idx, 0 ); 00401 } 00402 double& operator()( unsigned idx ) 00403 { 00404 return MsqMatrix< L, 1 >::operator()( idx, 0 ); 00405 } 00406 double operator()( unsigned idx ) const 00407 { 00408 return MsqMatrix< L, 1 >::operator()( idx, 0 ); 00409 } 00410 00411 MsqVector< L >& operator=( const MsqMatrix< L, 1 >& p_m ) 00412 { 00413 MsqMatrix< L, 1 >::operator=( p_m ); 00414 return *this; 00415 } 00416 }; 00417 00418 /**\brief A subclass of MsqMatrix that behaves more like the old Matrix3D class 00419 * 00420 * A MsqMartix that behaves more like the old Matrx3D class. 00421 * * Constructors are different--be careful. 00422 * * Adds operator[] for c-array style access. 00423 */ 00424 template < unsigned R, unsigned C > 00425 class MsqMatrixA : public MsqMatrix< R, C > 00426 { 00427 public: 00428 /** Initialize all elements to zero */ 00429 MsqMatrixA() 00430 { 00431 MsqMatrix< R, C >::set( 0 ); 00432 } 00433 /** Initialize all elements to passed value */ 00434 MsqMatrixA( double v ) 00435 { 00436 MsqMatrix< R, C >::set( v ); 00437 } 00438 MsqMatrixA( const double* v ) 00439 { 00440 MsqMatrix< R, C >::set( v ); 00441 } 00442 MsqMatrixA( const char* s ) 00443 { 00444 MsqMatrix< R, C >::set( s ); 00445 } 00446 MsqMatrixA( const std::string& s ) 00447 { 00448 MsqMatrix< R, C >::set( s ); 00449 } 00450 MsqMatrixA( const MsqMatrix< R, C >& m ) : MsqMatrix< R, C >( m ) {} 00451 MsqMatrixA( const MsqMatrix< R + 1, C + 1 >& m, unsigned r, unsigned c ) 00452 { 00453 MsqMatrix< R, C >::make_minor( m, r, c ); 00454 } 00455 00456 double* operator[]( unsigned idx ) 00457 { 00458 return MsqMatrix< R, C >::m + C * idx; 00459 } 00460 const double* operator[]( unsigned idx ) const 00461 { 00462 return MsqMatrix< R, C >::m + C * idx; 00463 } 00464 00465 MsqMatrixA< R, C >& operator=( const MsqMatrixA< R, C >& m ) 00466 { 00467 MsqMatrixA< R, C >::operator=( m ); 00468 return *this; 00469 } 00470 }; 00471 00472 template < unsigned R, unsigned C > 00473 inline void MsqMatrix< R, C >::set( const double v[R][C] ) 00474 { 00475 for( unsigned r = 0; r < R; ++r ) 00476 for( unsigned c = 0; c < C; ++c ) 00477 operator()( r, c ) = v[r][c]; 00478 } 00479 00480 template < unsigned R, unsigned C > 00481 inline void MsqMatrix< R, C >::diag( double v ) 00482 { 00483 // for (unsigned r = 0; r < R; ++r) 00484 // for (unsigned c = 0; c < C; ++c) 00485 // operator()(r,c) = (r == c) ? v : 0.0; 00486 00487 switch( R ) 00488 { 00489 default: 00490 for( unsigned r = 4; r < R; ++r ) 00491 switch( C ) 00492 { 00493 default: 00494 for( unsigned k = 4; k < C; ++k ) 00495 operator()( r, k ) = r == k ? v : 0.0; 00496 case 4: 00497 operator()( r, 3 ) = 0.0; 00498 case 3: 00499 operator()( r, 2 ) = 0.0; 00500 case 2: 00501 operator()( r, 1 ) = 0.0; 00502 case 1: 00503 operator()( r, 0 ) = 0.0; 00504 } 00505 case 4: 00506 switch( C ) 00507 { 00508 default: 00509 for( unsigned k = 4; k < C; ++k ) 00510 operator()( 3, k ) = 0.0; 00511 case 4: 00512 operator()( 3, 3 ) = v; 00513 case 3: 00514 operator()( 3, 2 ) = 0.0; 00515 case 2: 00516 operator()( 3, 1 ) = 0.0; 00517 case 1: 00518 operator()( 3, 0 ) = 0.0; 00519 } 00520 case 3: 00521 switch( C ) 00522 { 00523 default: 00524 for( unsigned k = 4; k < C; ++k ) 00525 operator()( 2, k ) = 0.0; 00526 case 4: 00527 operator()( 2, 3 ) = 0.0; 00528 case 3: 00529 operator()( 2, 2 ) = v; 00530 case 2: 00531 operator()( 2, 1 ) = 0.0; 00532 case 1: 00533 operator()( 2, 0 ) = 0.0; 00534 } 00535 case 2: 00536 switch( C ) 00537 { 00538 default: 00539 for( unsigned k = 4; k < C; ++k ) 00540 operator()( 1, k ) = 0.0; 00541 case 4: 00542 operator()( 1, 3 ) = 0.0; 00543 case 3: 00544 operator()( 1, 2 ) = 0.0; 00545 case 2: 00546 operator()( 1, 1 ) = v; 00547 case 1: 00548 operator()( 1, 0 ) = 0.0; 00549 } 00550 case 1: 00551 switch( C ) 00552 { 00553 default: 00554 for( unsigned k = 4; k < C; ++k ) 00555 operator()( 0, k ) = 0.0; 00556 case 4: 00557 operator()( 0, 3 ) = 0.0; 00558 case 3: 00559 operator()( 0, 2 ) = 0.0; 00560 case 2: 00561 operator()( 0, 1 ) = 0.0; 00562 case 1: 00563 operator()( 0, 0 ) = v; 00564 } 00565 } 00566 } 00567 00568 template < unsigned R, unsigned C > 00569 inline void MsqMatrix< R, C >::diag( const double* v ) 00570 { 00571 // for (unsigned r = 0; r < R; ++r) 00572 // for (unsigned c = 0; c < C; ++c) 00573 // operator()(r,c) = (r == c) ? v[r] : 0.0; 00574 00575 switch( R ) 00576 { 00577 default: 00578 for( unsigned r = 4; r < R; ++r ) 00579 switch( C ) 00580 { 00581 default: 00582 for( unsigned k = 4; k < C; ++k ) 00583 operator()( r, k ) = r == k ? v[r] : 0.0; 00584 case 4: 00585 operator()( r, 3 ) = 0.0; 00586 case 3: 00587 operator()( r, 2 ) = 0.0; 00588 case 2: 00589 operator()( r, 1 ) = 0.0; 00590 case 1: 00591 operator()( r, 0 ) = 0.0; 00592 } 00593 case 4: 00594 switch( C ) 00595 { 00596 default: 00597 for( unsigned k = 4; k < C; ++k ) 00598 operator()( 3, k ) = 0.0; 00599 case 4: 00600 operator()( 3, 3 ) = v[3]; 00601 case 3: 00602 operator()( 3, 2 ) = 0.0; 00603 case 2: 00604 operator()( 3, 1 ) = 0.0; 00605 case 1: 00606 operator()( 3, 0 ) = 0.0; 00607 } 00608 case 3: 00609 switch( C ) 00610 { 00611 default: 00612 for( unsigned k = 4; k < C; ++k ) 00613 operator()( 2, k ) = 0.0; 00614 case 4: 00615 operator()( 2, 3 ) = 0.0; 00616 case 3: 00617 operator()( 2, 2 ) = v[2]; 00618 case 2: 00619 operator()( 2, 1 ) = 0.0; 00620 case 1: 00621 operator()( 2, 0 ) = 0.0; 00622 } 00623 case 2: 00624 switch( C ) 00625 { 00626 default: 00627 for( unsigned k = 4; k < C; ++k ) 00628 operator()( 1, k ) = 0.0; 00629 case 4: 00630 operator()( 1, 3 ) = 0.0; 00631 case 3: 00632 operator()( 1, 2 ) = 0.0; 00633 case 2: 00634 operator()( 1, 1 ) = v[1]; 00635 case 1: 00636 operator()( 1, 0 ) = 0.0; 00637 } 00638 case 1: 00639 switch( C ) 00640 { 00641 default: 00642 for( unsigned k = 4; k < C; ++k ) 00643 operator()( 0, k ) = 0.0; 00644 case 4: 00645 operator()( 0, 3 ) = 0.0; 00646 case 3: 00647 operator()( 0, 2 ) = 0.0; 00648 case 2: 00649 operator()( 0, 1 ) = 0.0; 00650 case 1: 00651 operator()( 0, 0 ) = v[0]; 00652 } 00653 } 00654 } 00655 00656 /**\brief Extract minor of a matrix and assign to *this 00657 * 00658 * Given a matrix m, a row r and an column c, set *this to 00659 * the matrix that is m with row r and column c deleted. 00660 */ 00661 template < unsigned R, unsigned C > 00662 inline void MsqMatrix< R, C >::make_minor( const MsqMatrix< R + 1, C + 1 >& M, unsigned r, unsigned c ) 00663 { 00664 for( unsigned i = 0; i < r; ++i ) 00665 { 00666 for( unsigned j = 0; j < c; ++j ) 00667 operator()( i, j ) = M( i, j ); 00668 for( unsigned j = c; j < C; ++j ) 00669 operator()( i, j ) = M( i, j + 1 ); 00670 } 00671 for( unsigned i = r; i < R; ++i ) 00672 { 00673 for( unsigned j = 0; j < c; ++j ) 00674 operator()( i, j ) = M( i + 1, j ); 00675 for( unsigned j = c; j < C; ++j ) 00676 operator()( i, j ) = M( i + 1, j + 1 ); 00677 } 00678 } 00679 00680 template < unsigned R, unsigned C > 00681 inline void MsqMatrix< R, C >::set_row( unsigned r, const MsqMatrix< 1, C >& v ) 00682 { 00683 for( unsigned i = 0; i < C; ++i ) 00684 operator()( r, i ) = v( 0, i ); 00685 } 00686 template < unsigned R, unsigned C > 00687 inline void MsqMatrix< R, C >::add_row( unsigned r, const MsqMatrix< 1, C >& v ) 00688 { 00689 for( unsigned i = 0; i < C; ++i ) 00690 operator()( r, i ) += v( 0, i ); 00691 } 00692 template < unsigned R, unsigned C > 00693 inline void MsqMatrix< R, C >::set_row_transpose( unsigned r, const MsqMatrix< C, 1 >& v ) 00694 { 00695 for( unsigned i = 0; i < C; ++i ) 00696 operator()( r, i ) = v( i, 0 ); 00697 } 00698 template < unsigned R, unsigned C > 00699 inline void MsqMatrix< R, C >::set_rows( const MsqMatrix< 1, C >* v ) 00700 { 00701 for( unsigned r = 0; r < R; ++r ) 00702 set_row( r, v[r] ); 00703 } 00704 00705 template < unsigned R, unsigned C > 00706 inline void MsqMatrix< R, C >::set_column( unsigned c, const MsqMatrix< R, 1 >& v ) 00707 { 00708 for( unsigned i = 0; i < R; ++i ) 00709 operator()( i, c ) = v( i, 0 ); 00710 } 00711 template < unsigned R, unsigned C > 00712 inline void MsqMatrix< R, C >::add_column( unsigned c, const MsqMatrix< R, 1 >& v ) 00713 { 00714 for( unsigned i = 0; i < R; ++i ) 00715 operator()( i, c ) += v( i, 0 ); 00716 } 00717 template < unsigned R, unsigned C > 00718 inline void MsqMatrix< R, C >::set_column_transpose( unsigned c, const MsqMatrix< 1, R >& v ) 00719 { 00720 for( unsigned i = 0; i < R; ++i ) 00721 operator()( i, c ) = v( 0, i ); 00722 } 00723 template < unsigned R, unsigned C > 00724 inline void MsqMatrix< R, C >::set_columns( const MsqMatrix< R, 1 >* v ) 00725 { 00726 for( unsigned c = 0; c < C; ++c ) 00727 set_column( c, v[c] ); 00728 } 00729 00730 template < unsigned R, unsigned C > 00731 inline MsqMatrix< R, 1 > MsqMatrix< R, C >::column( unsigned c ) const 00732 { 00733 MsqMatrix< R, 1 > result; 00734 for( unsigned i = 0; i < R; ++i ) 00735 result( i, 0 ) = operator()( i, c ); 00736 return result; 00737 } 00738 00739 template < unsigned R, unsigned C > 00740 inline MsqMatrix< 1, R > MsqMatrix< R, C >::column_transpose( unsigned c ) const 00741 { 00742 MsqMatrix< 1, R > result; 00743 for( unsigned i = 0; i < R; ++i ) 00744 result( 0, i ) = operator()( i, c ); 00745 return result; 00746 } 00747 00748 /**\brief Set a subset of this matrix to some other matrix */ 00749 template < unsigned R1, unsigned C1, unsigned R2, unsigned C2 > 00750 inline void set_region( MsqMatrix< R1, C1 >& d, unsigned r, unsigned c, MsqMatrix< R2, C2 >& s ) 00751 { 00752 const unsigned rmax = r + R2 > R1 ? R1 : r + R2; 00753 const unsigned cmax = c + C2 > C1 ? C1 : c + C2; 00754 for( unsigned i = r; i < rmax; ++i ) 00755 for( unsigned j = c; j < cmax; ++j ) 00756 d( i, j ) = s( i - r, j - c ); 00757 } 00758 00759 template < unsigned R, unsigned C > 00760 inline MsqMatrix< R, C >& MsqMatrix< R, C >::assign_add_transpose( const MsqMatrix< C, R >& other ) 00761 { 00762 for( unsigned r = 0; r < R; ++r ) 00763 for( unsigned c = 0; c < C; ++c ) 00764 operator()( r, c ) += other( c, r ); 00765 return *this; 00766 } 00767 00768 template < unsigned R, unsigned C > 00769 inline MsqMatrix< R, C >& MsqMatrix< R, C >::assign_multiply_elements( const MsqMatrix< R, C >& other ) 00770 { 00771 for( unsigned i = 0; i < R * C; ++i ) 00772 m[i] *= other.data()[i]; 00773 return *this; 00774 } 00775 00776 template < unsigned R, unsigned C > 00777 inline MsqMatrix< R, C >& MsqMatrix< R, C >::assign_product( double s, const MsqMatrix< R, C >& other ) 00778 { 00779 for( unsigned i = 0; i < R * C; ++i ) 00780 m[i] = s * other.data()[i]; 00781 return *this; 00782 } 00783 00784 template < unsigned R, unsigned C > 00785 inline MsqMatrix< R, C >& MsqMatrix< R, C >::assign_add_product( double s, const MsqMatrix< R, C >& other ) 00786 { 00787 for( unsigned i = 0; i < R * C; ++i ) 00788 m[i] += s * other.data()[i]; 00789 return *this; 00790 } 00791 00792 template < unsigned R, unsigned C > 00793 inline MsqMatrix< R, C >& MsqMatrix< R, C >::operator+=( const MsqMatrix< R, C >& other ) 00794 { 00795 for( unsigned i = 0; i < R * C; ++i ) 00796 m[i] += other.data()[i]; 00797 return *this; 00798 } 00799 00800 template < unsigned R, unsigned C > 00801 inline MsqMatrix< R, C >& MsqMatrix< R, C >::operator-=( const MsqMatrix< R, C >& other ) 00802 { 00803 for( unsigned i = 0; i < R * C; ++i ) 00804 m[i] -= other.data()[i]; 00805 return *this; 00806 } 00807 00808 template < unsigned R, unsigned C > 00809 inline MsqMatrix< R, C >& MsqMatrix< R, C >::operator+=( double s ) 00810 { 00811 for( unsigned i = 0; i < R * C; ++i ) 00812 m[i] += s; 00813 return *this; 00814 } 00815 00816 template < unsigned R, unsigned C > 00817 inline MsqMatrix< R, C >& MsqMatrix< R, C >::operator-=( double s ) 00818 { 00819 for( unsigned i = 0; i < R * C; ++i ) 00820 m[i] -= s; 00821 return *this; 00822 } 00823 00824 template < unsigned R, unsigned C > 00825 inline MsqMatrix< R, C >& MsqMatrix< R, C >::operator*=( double s ) 00826 { 00827 for( unsigned i = 0; i < R * C; ++i ) 00828 m[i] *= s; 00829 return *this; 00830 } 00831 00832 template < unsigned R, unsigned C > 00833 inline MsqMatrix< R, C >& MsqMatrix< R, C >::operator/=( double s ) 00834 { 00835 for( unsigned i = 0; i < R * C; ++i ) 00836 m[i] /= s; 00837 return *this; 00838 } 00839 00840 template < unsigned R, unsigned C > 00841 inline MsqMatrix< R, C > operator-( const MsqMatrix< R, C >& m ) 00842 { 00843 MsqMatrix< R, C > result; 00844 for( unsigned i = 0; i < R * C; ++i ) 00845 result.data()[i] = -m.data()[i]; 00846 return result; 00847 } 00848 00849 template < unsigned R, unsigned C > 00850 inline MsqMatrix< R, C > operator+( const MsqMatrix< R, C >& m, double s ) 00851 { 00852 MsqMatrix< R, C > tmp( m ); 00853 tmp += s; 00854 return tmp; 00855 } 00856 00857 template < unsigned R, unsigned C > 00858 inline MsqMatrix< R, C > operator+( double s, const MsqMatrix< R, C >& m ) 00859 { 00860 MsqMatrix< R, C > tmp( m ); 00861 tmp += s; 00862 return tmp; 00863 } 00864 00865 template < unsigned R, unsigned C > 00866 inline MsqMatrix< R, C > operator+( const MsqMatrix< R, C >& A, const MsqMatrix< R, C >& B ) 00867 { 00868 MsqMatrix< R, C > tmp( A ); 00869 tmp += B; 00870 return tmp; 00871 } 00872 00873 template < unsigned R, unsigned C > 00874 inline MsqMatrix< R, C > operator-( const MsqMatrix< R, C >& m, double s ) 00875 { 00876 MsqMatrix< R, C > tmp( m ); 00877 tmp -= s; 00878 return tmp; 00879 } 00880 00881 template < unsigned R, unsigned C > 00882 inline MsqMatrix< R, C > operator-( double s, const MsqMatrix< R, C >& m ) 00883 { 00884 MsqMatrix< R, C > tmp( m ); 00885 tmp -= s; 00886 return tmp; 00887 } 00888 00889 template < unsigned R, unsigned C > 00890 inline MsqMatrix< R, C > operator-( const MsqMatrix< R, C >& A, const MsqMatrix< R, C >& B ) 00891 { 00892 MsqMatrix< R, C > tmp( A ); 00893 tmp -= B; 00894 return tmp; 00895 } 00896 00897 template < unsigned R, unsigned C > 00898 inline MsqMatrix< R, C > operator*( const MsqMatrix< R, C >& m, double s ) 00899 { 00900 MsqMatrix< R, C > tmp( m ); 00901 tmp *= s; 00902 return tmp; 00903 } 00904 00905 template < unsigned R, unsigned C > 00906 inline MsqMatrix< R, C > operator*( double s, const MsqMatrix< R, C >& m ) 00907 { 00908 MsqMatrix< R, C > tmp( m ); 00909 tmp *= s; 00910 return tmp; 00911 } 00912 00913 template < unsigned R, unsigned RC, unsigned C > 00914 inline double multiply_helper_result_val( unsigned r, unsigned c, const MsqMatrix< R, RC >& A, 00915 const MsqMatrix< RC, C >& B ) 00916 { 00917 double tmp = A( r, 0 ) * B( 0, c ); 00918 switch( RC ) 00919 { 00920 default: 00921 for( unsigned k = 6; k < RC; ++k ) 00922 tmp += A( r, k ) * B( k, c ); 00923 case 6: 00924 tmp += A( r, 5 ) * B( 5, c ); 00925 case 5: 00926 tmp += A( r, 4 ) * B( 4, c ); 00927 case 4: 00928 tmp += A( r, 3 ) * B( 3, c ); 00929 case 3: 00930 tmp += A( r, 2 ) * B( 2, c ); 00931 case 2: 00932 tmp += A( r, 1 ) * B( 1, c ); 00933 case 1:; 00934 } 00935 return tmp; 00936 } 00937 00938 template < unsigned R, unsigned RC, unsigned C > 00939 inline MsqMatrix< R, C > operator*( const MsqMatrix< R, RC >& A, const MsqMatrix< RC, C >& B ) 00940 { 00941 // MsqMatrix<R,C> result(0.0); 00942 // for (unsigned i = 0; i < R; ++i) 00943 // for (unsigned j = 0; j < C; ++j) 00944 // for (unsigned k = 0; k < RC; ++k) 00945 // result(i,j) += A(i,k) * B(k,j); 00946 00947 MsqMatrix< R, C > result; 00948 for( unsigned r = 0; r < R; ++r ) 00949 for( unsigned c = 0; c < C; ++c ) 00950 result( r, c ) = multiply_helper_result_val( r, c, A, B ); 00951 00952 return result; 00953 } 00954 00955 template < unsigned R, unsigned C > 00956 inline MsqMatrix< R, C > operator/( const MsqMatrix< R, C >& m, double s ) 00957 { 00958 MsqMatrix< R, C > tmp( m ); 00959 tmp /= s; 00960 return tmp; 00961 } 00962 00963 template < unsigned RC > 00964 inline double cofactor( const MsqMatrix< RC, RC >& m, unsigned r, unsigned c ) 00965 { 00966 const double sign[] = { 1.0, -1.0 }; 00967 return sign[( r + c ) % 2] * det( MsqMatrix< RC - 1, RC - 1 >( m, r, c ) ); 00968 } 00969 00970 template < unsigned RC > 00971 inline double det( const MsqMatrix< RC, RC >& m ) 00972 { 00973 double result = 0.0; 00974 for( unsigned i = 0; i < RC; ++i ) 00975 result += m( 0, i ) * cofactor< RC >( m, 0, i ); 00976 return result; 00977 } 00978 00979 // inline 00980 // double det( const MsqMatrix<1,1>& m ) 00981 // { return m(0,0); } 00982 00983 inline double det( const MsqMatrix< 2, 2 >& m ) 00984 { 00985 return m( 0, 0 ) * m( 1, 1 ) - m( 0, 1 ) * m( 1, 0 ); 00986 } 00987 00988 inline double det( const MsqMatrix< 3, 3 >& m ) 00989 { 00990 return m( 0, 0 ) * ( m( 1, 1 ) * m( 2, 2 ) - m( 2, 1 ) * m( 1, 2 ) ) + 00991 m( 0, 1 ) * ( m( 2, 0 ) * m( 1, 2 ) - m( 1, 0 ) * m( 2, 2 ) ) + 00992 m( 0, 2 ) * ( m( 1, 0 ) * m( 2, 1 ) - m( 2, 0 ) * m( 1, 1 ) ); 00993 } 00994 00995 inline MsqMatrix< 2, 2 > adj( const MsqMatrix< 2, 2 >& m ) 00996 { 00997 MsqMatrix< 2, 2 > result; 00998 result( 0, 0 ) = m( 1, 1 ); 00999 result( 0, 1 ) = -m( 0, 1 ); 01000 result( 1, 0 ) = -m( 1, 0 ); 01001 result( 1, 1 ) = m( 0, 0 ); 01002 return result; 01003 } 01004 01005 inline MsqMatrix< 2, 2 > transpose_adj( const MsqMatrix< 2, 2 >& m ) 01006 { 01007 MsqMatrix< 2, 2 > result; 01008 result( 0, 0 ) = m( 1, 1 ); 01009 result( 1, 0 ) = -m( 0, 1 ); 01010 result( 0, 1 ) = -m( 1, 0 ); 01011 result( 1, 1 ) = m( 0, 0 ); 01012 return result; 01013 } 01014 01015 inline MsqMatrix< 3, 3 > adj( const MsqMatrix< 3, 3 >& m ) 01016 { 01017 MsqMatrix< 3, 3 > result; 01018 01019 result( 0, 0 ) = m( 1, 1 ) * m( 2, 2 ) - m( 1, 2 ) * m( 2, 1 ); 01020 result( 0, 1 ) = m( 0, 2 ) * m( 2, 1 ) - m( 0, 1 ) * m( 2, 2 ); 01021 result( 0, 2 ) = m( 0, 1 ) * m( 1, 2 ) - m( 0, 2 ) * m( 1, 1 ); 01022 01023 result( 1, 0 ) = m( 1, 2 ) * m( 2, 0 ) - m( 1, 0 ) * m( 2, 2 ); 01024 result( 1, 1 ) = m( 0, 0 ) * m( 2, 2 ) - m( 0, 2 ) * m( 2, 0 ); 01025 result( 1, 2 ) = m( 0, 2 ) * m( 1, 0 ) - m( 0, 0 ) * m( 1, 2 ); 01026 01027 result( 2, 0 ) = m( 1, 0 ) * m( 2, 1 ) - m( 1, 1 ) * m( 2, 0 ); 01028 result( 2, 1 ) = m( 0, 1 ) * m( 2, 0 ) - m( 0, 0 ) * m( 2, 1 ); 01029 result( 2, 2 ) = m( 0, 0 ) * m( 1, 1 ) - m( 0, 1 ) * m( 1, 0 ); 01030 01031 return result; 01032 } 01033 01034 inline MsqMatrix< 3, 3 > transpose_adj( const MsqMatrix< 3, 3 >& m ) 01035 { 01036 MsqMatrix< 3, 3 > result; 01037 01038 result( 0, 0 ) = m( 1, 1 ) * m( 2, 2 ) - m( 1, 2 ) * m( 2, 1 ); 01039 result( 0, 1 ) = m( 1, 2 ) * m( 2, 0 ) - m( 1, 0 ) * m( 2, 2 ); 01040 result( 0, 2 ) = m( 1, 0 ) * m( 2, 1 ) - m( 1, 1 ) * m( 2, 0 ); 01041 01042 result( 1, 0 ) = m( 0, 2 ) * m( 2, 1 ) - m( 0, 1 ) * m( 2, 2 ); 01043 result( 1, 1 ) = m( 0, 0 ) * m( 2, 2 ) - m( 0, 2 ) * m( 2, 0 ); 01044 result( 1, 2 ) = m( 0, 1 ) * m( 2, 0 ) - m( 0, 0 ) * m( 2, 1 ); 01045 01046 result( 2, 0 ) = m( 0, 1 ) * m( 1, 2 ) - m( 0, 2 ) * m( 1, 1 ); 01047 result( 2, 1 ) = m( 0, 2 ) * m( 1, 0 ) - m( 0, 0 ) * m( 1, 2 ); 01048 result( 2, 2 ) = m( 0, 0 ) * m( 1, 1 ) - m( 0, 1 ) * m( 1, 0 ); 01049 01050 return result; 01051 } 01052 01053 template < unsigned R, unsigned C > 01054 inline MsqMatrix< R, C > inverse( const MsqMatrix< R, C >& m ) 01055 { 01056 return adj( m ) * ( 1.0 / det( m ) ); 01057 } 01058 01059 template < unsigned R, unsigned C > 01060 inline MsqMatrix< R, C > transpose( const MsqMatrix< C, R >& m ) 01061 { 01062 MsqMatrix< R, C > result; 01063 for( unsigned r = 0; r < R; ++r ) 01064 for( unsigned c = 0; c < C; ++c ) 01065 result( r, c ) = m( c, r ); 01066 return result; 01067 } 01068 /* 01069 template <unsigned R> inline 01070 const MsqMatrix<R,1>& transpose( const MsqMatrix<1,R>& m ) 01071 { return *reinterpret_cast<const MsqMatrix<R,1>*>(&m); } 01072 01073 template <unsigned C> inline 01074 const MsqMatrix<1,C>& transpose( const MsqMatrix<C,1>& m ) 01075 { return *reinterpret_cast<const MsqMatrix<1,C>*>(&m); } 01076 */ 01077 template < unsigned RC > 01078 inline double trace( const MsqMatrix< RC, RC >& m ) 01079 { 01080 double result = m( 0, 0 ); 01081 for( unsigned i = 1; i < RC; ++i ) 01082 result += m( i, i ); 01083 return result; 01084 } 01085 01086 template < unsigned R, unsigned C > 01087 inline double sqr_Frobenius( const MsqMatrix< R, C >& m ) 01088 { 01089 double result = *m.data() * *m.data(); 01090 for( unsigned i = 1; i < R * C; ++i ) 01091 result += m.data()[i] * m.data()[i]; 01092 return result; 01093 } 01094 01095 template < unsigned R, unsigned C > 01096 inline double Frobenius( const MsqMatrix< R, C >& m ) 01097 { 01098 return std::sqrt( sqr_Frobenius< R, C >( m ) ); 01099 } 01100 01101 template < unsigned R, unsigned C > 01102 inline bool operator==( const MsqMatrix< R, C >& A, const MsqMatrix< R, C >& B ) 01103 { 01104 for( unsigned i = 0; i < R * C; ++i ) 01105 if( A.data()[i] != B.data()[i] ) return false; 01106 return true; 01107 } 01108 01109 template < unsigned R, unsigned C > 01110 inline bool operator!=( const MsqMatrix< R, C >& A, const MsqMatrix< R, C >& B ) 01111 { 01112 return !( A == B ); 01113 } 01114 01115 template < unsigned R, unsigned C > 01116 inline std::ostream& operator<<( std::ostream& str, const MsqMatrix< R, C >& m ) 01117 { 01118 str << m.data()[0]; 01119 for( unsigned i = 1; i < R * C; ++i ) 01120 str << ' ' << m.data()[i]; 01121 return str; 01122 } 01123 01124 template < unsigned R, unsigned C > 01125 inline std::istream& operator>>( std::istream& str, MsqMatrix< R, C >& m ) 01126 { 01127 for( unsigned i = 0; i < R * C; ++i ) 01128 str >> m.data()[i]; 01129 return str; 01130 } 01131 01132 template < unsigned R > 01133 inline double sqr_length( const MsqMatrix< R, 1 >& v ) 01134 { 01135 return sqr_Frobenius( v ); 01136 } 01137 01138 template < unsigned C > 01139 inline double sqr_length( const MsqMatrix< 1, C >& v ) 01140 { 01141 return sqr_Frobenius( v ); 01142 } 01143 01144 template < unsigned R > 01145 inline double length( const MsqMatrix< R, 1 >& v ) 01146 { 01147 return Frobenius( v ); 01148 } 01149 01150 template < unsigned C > 01151 inline double length( const MsqMatrix< 1, C >& v ) 01152 { 01153 return Frobenius( v ); 01154 } 01155 01156 template < unsigned R, unsigned C > 01157 inline double inner_product( const MsqMatrix< R, C >& m1, const MsqMatrix< R, C >& m2 ) 01158 { 01159 double result = 0.0; 01160 for( unsigned r = 0; r < R; ++r ) 01161 for( unsigned c = 0; c < C; ++c ) 01162 result += m1( r, c ) * m2( r, c ); 01163 return result; 01164 } 01165 01166 template < unsigned R, unsigned C > 01167 inline MsqMatrix< R, C > outer( const MsqMatrix< R, 1 >& v1, const MsqMatrix< C, 1 >& v2 ) 01168 { 01169 MsqMatrix< R, C > result; 01170 for( unsigned r = 0; r < R; ++r ) 01171 for( unsigned c = 0; c < C; ++c ) 01172 result( r, c ) = v1( r, 0 ) * v2( c, 0 ); 01173 return result; 01174 } 01175 01176 template < unsigned R, unsigned C > 01177 inline MsqMatrix< R, C > outer( const MsqMatrix< 1, R >& v1, const MsqMatrix< 1, C >& v2 ) 01178 { 01179 MsqMatrix< R, C > result; 01180 for( unsigned r = 0; r < R; ++r ) 01181 for( unsigned c = 0; c < C; ++c ) 01182 result( r, c ) = v1( 0, r ) * v2( 0, c ); 01183 return result; 01184 } 01185 01186 inline double vector_product( const MsqMatrix< 2, 1 >& v1, const MsqMatrix< 2, 1 >& v2 ) 01187 { 01188 return v1( 0, 0 ) * v2( 1, 0 ) - v1( 1, 0 ) * v2( 0, 0 ); 01189 } 01190 01191 inline double vector_product( const MsqMatrix< 1, 2 >& v1, const MsqMatrix< 1, 2 >& v2 ) 01192 { 01193 return v1( 0, 0 ) * v2( 0, 1 ) - v1( 0, 1 ) * v2( 0, 0 ); 01194 } 01195 01196 inline MsqMatrix< 3, 1 > vector_product( const MsqMatrix< 3, 1 >& a, const MsqMatrix< 3, 1 >& b ) 01197 { 01198 MsqMatrix< 3, 1 > result; 01199 result( 0, 0 ) = a( 1, 0 ) * b( 2, 0 ) - a( 2, 0 ) * b( 1, 0 ); 01200 result( 1, 0 ) = a( 2, 0 ) * b( 0, 0 ) - a( 0, 0 ) * b( 2, 0 ); 01201 result( 2, 0 ) = a( 0, 0 ) * b( 1, 0 ) - a( 1, 0 ) * b( 0, 0 ); 01202 return result; 01203 } 01204 01205 inline MsqMatrix< 1, 3 > vector_product( const MsqMatrix< 1, 3 >& a, const MsqMatrix< 1, 3 >& b ) 01206 { 01207 MsqMatrix< 1, 3 > result; 01208 result( 0, 0 ) = a( 0, 1 ) * b( 0, 2 ) - a( 0, 2 ) * b( 0, 1 ); 01209 result( 0, 1 ) = a( 0, 2 ) * b( 0, 0 ) - a( 0, 0 ) * b( 0, 2 ); 01210 result( 0, 2 ) = a( 0, 0 ) * b( 0, 1 ) - a( 0, 1 ) * b( 0, 0 ); 01211 return result; 01212 } 01213 01214 template < unsigned R, unsigned C > 01215 inline double operator%( const MsqMatrix< R, C >& v1, const MsqMatrix< R, C >& v2 ) 01216 { 01217 return inner_product( v1, v2 ); 01218 } 01219 01220 inline double operator*( const MsqMatrix< 2, 1 >& v1, const MsqMatrix< 2, 1 >& v2 ) 01221 { 01222 return vector_product( v1, v2 ); 01223 } 01224 01225 inline double operator*( const MsqMatrix< 1, 2 >& v1, const MsqMatrix< 1, 2 >& v2 ) 01226 { 01227 return vector_product( v1, v2 ); 01228 } 01229 01230 inline MsqMatrix< 3, 1 > operator*( const MsqMatrix< 3, 1 >& v1, const MsqMatrix< 3, 1 >& v2 ) 01231 { 01232 return vector_product( v1, v2 ); 01233 } 01234 01235 inline MsqMatrix< 1, 3 > operator*( const MsqMatrix< 1, 3 >& v1, const MsqMatrix< 1, 3 >& v2 ) 01236 { 01237 return vector_product( v1, v2 ); 01238 } 01239 01240 /** Compute QR factorization of A */ 01241 inline void QR( const MsqMatrix< 3, 3 >& A, MsqMatrix< 3, 3 >& Q, MsqMatrix< 3, 3 >& R ) 01242 { 01243 Q = A; 01244 01245 R( 0, 0 ) = sqrt( Q( 0, 0 ) * Q( 0, 0 ) + Q( 1, 0 ) * Q( 1, 0 ) + Q( 2, 0 ) * Q( 2, 0 ) ); 01246 double temp_dbl = 1.0 / R( 0, 0 ); 01247 R( 1, 0 ) = 0.0; 01248 R( 2, 0 ) = 0.0; 01249 Q( 0, 0 ) *= temp_dbl; 01250 Q( 1, 0 ) *= temp_dbl; 01251 Q( 2, 0 ) *= temp_dbl; 01252 01253 R( 0, 1 ) = Q( 0, 0 ) * Q( 0, 1 ) + Q( 1, 0 ) * Q( 1, 1 ) + Q( 2, 0 ) * Q( 2, 1 ); 01254 Q( 0, 1 ) -= Q( 0, 0 ) * R( 0, 1 ); 01255 Q( 1, 1 ) -= Q( 1, 0 ) * R( 0, 1 ); 01256 Q( 2, 1 ) -= Q( 2, 0 ) * R( 0, 1 ); 01257 01258 R( 0, 2 ) = Q( 0, 0 ) * Q( 0, 2 ) + Q( 1, 0 ) * Q( 1, 2 ) + Q( 2, 0 ) * Q( 2, 2 ); 01259 Q( 0, 2 ) -= Q( 0, 0 ) * R( 0, 2 ); 01260 Q( 1, 2 ) -= Q( 1, 0 ) * R( 0, 2 ); 01261 Q( 2, 2 ) -= Q( 2, 0 ) * R( 0, 2 ); 01262 01263 R( 1, 1 ) = sqrt( Q( 0, 1 ) * Q( 0, 1 ) + Q( 1, 1 ) * Q( 1, 1 ) + Q( 2, 1 ) * Q( 2, 1 ) ); 01264 temp_dbl = 1.0 / R( 1, 1 ); 01265 R( 2, 1 ) = 0.0; 01266 Q( 0, 1 ) *= temp_dbl; 01267 Q( 1, 1 ) *= temp_dbl; 01268 Q( 2, 1 ) *= temp_dbl; 01269 01270 R( 1, 2 ) = Q( 0, 1 ) * Q( 0, 2 ) + Q( 1, 1 ) * Q( 1, 2 ) + Q( 2, 1 ) * Q( 2, 2 ); 01271 Q( 0, 2 ) -= Q( 0, 1 ) * R( 1, 2 ); 01272 Q( 1, 2 ) -= Q( 1, 1 ) * R( 1, 2 ); 01273 Q( 2, 2 ) -= Q( 2, 1 ) * R( 1, 2 ); 01274 01275 R( 2, 2 ) = sqrt( Q( 0, 2 ) * Q( 0, 2 ) + Q( 1, 2 ) * Q( 1, 2 ) + Q( 2, 2 ) * Q( 2, 2 ) ); 01276 temp_dbl = 1.0 / R( 2, 2 ); 01277 Q( 0, 2 ) *= temp_dbl; 01278 Q( 1, 2 ) *= temp_dbl; 01279 Q( 2, 2 ) *= temp_dbl; 01280 } 01281 01282 /** Compute QR factorization of A */ 01283 inline void QR( const MsqMatrix< 2, 2 >& A, MsqMatrix< 2, 2 >& Q, MsqMatrix< 2, 2 >& R ) 01284 { 01285 R( 0, 0 ) = std::sqrt( A( 0, 0 ) * A( 0, 0 ) + A( 1, 0 ) * A( 1, 0 ) ); 01286 const double r0inv = 1.0 / R( 0, 0 ); 01287 Q( 0, 0 ) = Q( 1, 1 ) = A( 0, 0 ) * r0inv; 01288 Q( 1, 0 ) = A( 1, 0 ) * r0inv; 01289 Q( 0, 1 ) = -Q( 1, 0 ); 01290 R( 0, 1 ) = r0inv * ( A( 0, 0 ) * A( 0, 1 ) + A( 1, 0 ) * A( 1, 1 ) ); 01291 R( 1, 1 ) = r0inv * ( A( 0, 0 ) * A( 1, 1 ) - A( 0, 1 ) * A( 1, 0 ) ); 01292 R( 1, 0 ) = 0.0; 01293 } 01294 01295 } // namespace MBMesquite 01296 01297 #endif