MOAB: Mesh Oriented datABase
(version 5.2.1)
|
00001 /* ***************************************************************** 00002 MESQUITE -- The Mesh Quality Improvement Toolkit 00003 00004 Copyright 2004 Sandia Corporation and Argonne National 00005 Laboratory. Under the terms of Contract DE-AC04-94AL85000 00006 with Sandia Corporation, the U.S. Government retains certain 00007 rights in 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 diachin2@llnl.gov, djmelan@sandia.gov, mbrewer@sandia.gov, 00024 pknupp@sandia.gov, tleurent@mcs.anl.gov, tmunson@mcs.anl.gov 00025 00026 ***************************************************************** */ 00027 // 00028 // AUTHOR: Thomas Leurent <tleurent@mcs.anl.gov> 00029 // ORG: Argonne National Laboratory 00030 // E-MAIL: tleurent@mcs.anl.gov 00031 // 00032 // ORIG-DATE: 18-Dec-02 at 11:08:22 00033 // LAST-MOD: 27-May-04 at 14:48:56 by Thomas Leurent 00034 // 00035 // DESCRIPTION: 00036 // ============ 00037 /*! \file Matrix3D.hpp 00038 00039 3*3 Matric class, row-oriented, 0-based [i][j] indexing. 00040 00041 \author Thomas Leurent 00042 00043 */ 00044 // DESCRIP-END. 00045 // 00046 00047 #ifndef Matrix3D_hpp 00048 #define Matrix3D_hpp 00049 00050 #include <iostream> 00051 #include <sstream> 00052 #include <cstdlib> 00053 00054 #include "Mesquite.hpp" 00055 #include "Vector3D.hpp" 00056 #include "SymMatrix3D.hpp" 00057 00058 namespace MBMesquite 00059 { 00060 00061 /*! \class Matrix3D 00062 \brief 3*3 Matric class, row-oriented, 0-based [i][j] indexing. 00063 00064 Since the size of the object is fixed at compile time, the Matrix3D 00065 object is as fast as a double[9] array. 00066 */ 00067 class MESQUITE_EXPORT Matrix3D 00068 { 00069 protected: 00070 double v_[9]; 00071 00072 void copy( const double* v ) 00073 { 00074 v_[0] = v[0]; 00075 v_[1] = v[1]; 00076 v_[2] = v[2]; 00077 v_[3] = v[3]; 00078 v_[4] = v[4]; 00079 v_[5] = v[5]; 00080 v_[6] = v[6]; 00081 v_[7] = v[7]; 00082 v_[8] = v[8]; 00083 } 00084 00085 void set( double val ) 00086 { 00087 v_[0] = val; 00088 v_[1] = val; 00089 v_[2] = val; 00090 v_[3] = val; 00091 v_[4] = val; 00092 v_[5] = val; 00093 v_[6] = val; 00094 v_[7] = val; 00095 v_[8] = val; 00096 } 00097 00098 inline void set_values( const char* s ); 00099 00100 public: 00101 // constructors 00102 //! Default constructor sets all entries to 0. 00103 Matrix3D() 00104 { 00105 zero(); 00106 } 00107 00108 Matrix3D( const Matrix3D& A ) 00109 { 00110 copy( A.v_ ); 00111 } 00112 00113 //! sets all entries of the matrix to value. 00114 Matrix3D( double value ) 00115 { 00116 set( value ); 00117 } 00118 00119 Matrix3D( double a00, double a01, double a02, double a10, double a11, double a12, double a20, double a21, 00120 double a22 ) 00121 { 00122 v_[0] = a00; 00123 v_[1] = a01; 00124 v_[2] = a02; 00125 v_[3] = a10; 00126 v_[4] = a11; 00127 v_[5] = a12; 00128 v_[6] = a20; 00129 v_[7] = a21; 00130 v_[8] = a22; 00131 } 00132 00133 Matrix3D( const Vector3D& col1, const Vector3D& col2, const Vector3D& col3 ) 00134 { 00135 set_column( 0, col1 ); 00136 set_column( 1, col2 ); 00137 set_column( 2, col3 ); 00138 } 00139 00140 Matrix3D( double radians, const Vector3D& axis ) 00141 { 00142 Vector3D v( axis ); 00143 v.normalize(); 00144 const double c = std::cos( radians ); 00145 const double s = std::sin( radians ); 00146 v_[0] = c + ( 1.0 - c ) * v[0] * v[0]; 00147 v_[1] = -v[2] * s + ( 1.0 - c ) * v[0] * v[1]; 00148 v_[2] = v[1] * s + ( 1.0 - c ) * v[0] * v[2]; 00149 v_[3] = v[2] * s + ( 1.0 - c ) * v[0] * v[1]; 00150 v_[4] = c + ( 1.0 - c ) * v[1] * v[1]; 00151 v_[5] = -v[0] * s + ( 1.0 - c ) * v[1] * v[2]; 00152 v_[6] = -v[1] * s + ( 1.0 - c ) * v[0] * v[2]; 00153 v_[7] = v[0] * s + ( 1.0 - c ) * v[1] * v[2]; 00154 v_[8] = c + ( 1.0 - c ) * v[2] * v[2]; 00155 } 00156 00157 //! sets matrix entries to values in array. 00158 //! \param v is an array of 9 doubles. 00159 Matrix3D( const double* v ) 00160 { 00161 copy( v ); 00162 } 00163 00164 //! for test purposes, matrices can be instantiated as 00165 //! \code Matrix3D A("3 2 1 4 5 6 9 8 7"); \endcode 00166 Matrix3D( const char* s ) 00167 { 00168 set_values( s ); 00169 } 00170 00171 Matrix3D( const SymMatrix3D& m ) 00172 { 00173 *this = m; 00174 } 00175 00176 // destructor 00177 ~Matrix3D() {} 00178 00179 // assignments 00180 Matrix3D& operator=( const Matrix3D& A ) 00181 { 00182 copy( A.v_ ); 00183 return *this; 00184 } 00185 00186 Matrix3D& operator=( const SymMatrix3D& m ) 00187 { 00188 v_[0] = m[0]; 00189 v_[1] = v_[3] = m[1]; 00190 v_[2] = v_[6] = m[2]; 00191 v_[4] = m[3]; 00192 v_[5] = v_[7] = m[4]; 00193 v_[8] = m[5]; 00194 return *this; 00195 } 00196 00197 Matrix3D& operator=( double scalar ) 00198 { 00199 set( scalar ); 00200 return *this; 00201 } 00202 00203 //! for test purposes, matrices can be assigned as follows 00204 //! \code A = "3 2 1 4 5 6 9 8 7"; \endcode 00205 Matrix3D& operator=( const char* s ) 00206 { 00207 set_values( s ); 00208 return *this; 00209 } 00210 00211 //! Sets all entries to zero (more efficient than assignement). 00212 void zero() 00213 { 00214 v_[0] = 0.; 00215 v_[1] = 0.; 00216 v_[2] = 0.; 00217 v_[3] = 0.; 00218 v_[4] = 0.; 00219 v_[5] = 0.; 00220 v_[6] = 0.; 00221 v_[7] = 0.; 00222 v_[8] = 0.; 00223 } 00224 00225 void identity() 00226 { 00227 v_[0] = 1.; 00228 v_[1] = 0.; 00229 v_[2] = 0.; 00230 v_[3] = 0.; 00231 v_[4] = 1.; 00232 v_[5] = 0.; 00233 v_[6] = 0.; 00234 v_[7] = 0.; 00235 v_[8] = 1.; 00236 } 00237 00238 //! Sets column j (0, 1 or 2) to Vector3D c. 00239 void set_column( int j, const Vector3D& c ) 00240 { 00241 v_[0 + j] = c[0]; 00242 v_[3 + j] = c[1]; 00243 v_[6 + j] = c[2]; 00244 } 00245 00246 //! returns the column length -- i is 0-based. 00247 double column_length( int i ) const 00248 { 00249 return sqrt( v_[0 + i] * v_[0 + i] + v_[3 + i] * v_[3 + i] + v_[6 + i] * v_[6 + i] ); 00250 } 00251 00252 double sub_det( int r, int c ) const 00253 { 00254 int r1 = 3 * ( ( r + 1 ) % 3 ); 00255 int r2 = 3 * ( ( r + 2 ) % 3 ); 00256 int c1 = ( ( c + 1 ) % 3 ); 00257 int c2 = ( ( c + 2 ) % 3 ); 00258 return v_[r1 + c1] * v_[r2 + c2] - v_[r2 + c1] * v_[r1 + c2]; 00259 } 00260 00261 // Matrix Operators 00262 friend bool operator==( const Matrix3D& lhs, const Matrix3D& rhs ); 00263 friend bool operator!=( const Matrix3D& lhs, const Matrix3D& rhs ); 00264 friend Matrix3D operator-( const Matrix3D& A ); 00265 friend double Frobenius_2( const Matrix3D& A ); 00266 friend Matrix3D transpose( const Matrix3D& A ); 00267 inline Matrix3D& transpose(); 00268 friend const Matrix3D operator+( const Matrix3D& A, const Matrix3D& B ); 00269 friend const Matrix3D operator-( const Matrix3D& A, const Matrix3D& B ); 00270 friend const Matrix3D operator*( const Matrix3D& A, const Matrix3D& B ); 00271 inline Matrix3D& equal_mult_elem( const Matrix3D& A ); 00272 friend const Matrix3D mult_element( const Matrix3D& A, const Matrix3D& B ); 00273 inline Matrix3D& assign_product( const Matrix3D& A, const Matrix3D& B ); 00274 friend void matmult( Matrix3D& C, const Matrix3D& A, const Matrix3D& B ); 00275 friend const Vector3D operator*( const Matrix3D& A, const Vector3D& x ); 00276 friend const Vector3D operator*( const Vector3D& x, const Matrix3D& A ); 00277 const Matrix3D operator*( double s ) const; 00278 friend const Matrix3D operator*( double s, const Matrix3D& A ); 00279 void operator+=( const Matrix3D& rhs ); 00280 void operator+=( const SymMatrix3D& rhs ); 00281 void operator-=( const Matrix3D& rhs ); 00282 void operator-=( const SymMatrix3D& rhs ); 00283 void operator*=( double s ); 00284 friend Matrix3D plus_transpose( const Matrix3D& A, const Matrix3D& B ); 00285 Matrix3D& plus_transpose_equal( const Matrix3D& B ); 00286 Matrix3D& outer_product( const Vector3D& v1, const Vector3D& v2 ); 00287 void fill_lower_triangle(); 00288 00289 //! \f$ v = A*x \f$ 00290 friend void eqAx( Vector3D& v, const Matrix3D& A, const Vector3D& x ); 00291 //! \f$ v += A*x \f$ 00292 friend void plusEqAx( Vector3D& v, const Matrix3D& A, const Vector3D& x ); 00293 friend void eqTransAx( Vector3D& v, const Matrix3D& A, const Vector3D& x ); 00294 //! \f$ v += A^T*x \f$ 00295 friend void plusEqTransAx( Vector3D& v, const Matrix3D& A, const Vector3D& x ); 00296 00297 //! \f$ B += a*A \f$ 00298 friend void plusEqaA( Matrix3D& B, const double a, const Matrix3D& A ); 00299 00300 //! determinant of matrix A, det(A). 00301 friend double det( const Matrix3D& A ); 00302 00303 //! \f$ B = A^{-1} \f$ 00304 friend void inv( Matrix3D& B, const Matrix3D& A ); 00305 00306 //! \f$ B *= A^{-1} \f$ 00307 friend void timesInvA( Matrix3D& B, const Matrix3D& A ); 00308 00309 //! \f$ Q*R = A \f$ 00310 friend void QR( Matrix3D& Q, Matrix3D& R, const Matrix3D& A ); 00311 00312 size_t num_rows() const 00313 { 00314 return 3; 00315 } 00316 size_t num_cols() const 00317 { 00318 return 3; 00319 } 00320 00321 //! returns a pointer to a row. 00322 inline double* operator[]( unsigned i ) 00323 { 00324 return v_ + 3 * i; 00325 } 00326 00327 //! returns a pointer to a row. 00328 inline const double* operator[]( unsigned i ) const 00329 { 00330 return v_ + 3 * i; 00331 } 00332 00333 inline double& operator()( unsigned short r, unsigned short c ) 00334 { 00335 return v_[3 * r + c]; 00336 } 00337 inline double operator()( unsigned short r, unsigned short c ) const 00338 { 00339 return v_[3 * r + c]; 00340 } 00341 00342 inline Vector3D row( unsigned r ) const 00343 { 00344 return Vector3D( v_ + 3 * r ); 00345 } 00346 00347 inline Vector3D column( unsigned c ) const 00348 { 00349 return Vector3D( v_[c], v_[c + 3], v_[c + 6] ); 00350 } 00351 00352 inline bool positive_definite() const; 00353 00354 inline SymMatrix3D upper() const 00355 { 00356 return SymMatrix3D( v_[0], v_[1], v_[2], v_[4], v_[5], v_[8] ); 00357 } 00358 inline SymMatrix3D lower() const 00359 { 00360 return SymMatrix3D( v_[0], v_[3], v_[6], v_[4], v_[7], v_[8] ); 00361 } 00362 }; 00363 00364 /* *********** I/O **************/ 00365 00366 inline std::ostream& operator<<( std::ostream& s, const Matrix3D& A ) 00367 { 00368 for( size_t i = 0; i < 3; ++i ) 00369 { 00370 for( size_t j = 0; j < 3; ++j ) 00371 s << A[i][j] << " "; 00372 s << "\n"; 00373 } 00374 return s; 00375 } 00376 00377 inline std::istream& operator>>( std::istream& s, Matrix3D& A ) 00378 { 00379 for( size_t i = 0; i < 3; i++ ) 00380 for( size_t j = 0; j < 3; j++ ) 00381 { 00382 s >> A[i][j]; 00383 } 00384 return s; 00385 } 00386 00387 void Matrix3D::set_values( const char* s ) 00388 { 00389 std::istringstream ins( s ); 00390 ins >> *this; 00391 } 00392 00393 // *********** matrix operators ******************* 00394 00395 // comparison functions 00396 inline bool operator==( const Matrix3D& lhs, const Matrix3D& rhs ) 00397 { 00398 return lhs.v_[0] == rhs.v_[0] && lhs.v_[1] == rhs.v_[1] && lhs.v_[2] == rhs.v_[2] && lhs.v_[3] == rhs.v_[3] && 00399 lhs.v_[4] == rhs.v_[4] && lhs.v_[5] == rhs.v_[5] && lhs.v_[6] == rhs.v_[6] && lhs.v_[7] == rhs.v_[7] && 00400 lhs.v_[8] == rhs.v_[8]; 00401 } 00402 inline bool operator!=( const Matrix3D& lhs, const Matrix3D& rhs ) 00403 { 00404 return !( lhs == rhs ); 00405 } 00406 00407 inline Matrix3D operator-( const Matrix3D& A ) 00408 { 00409 return Matrix3D( -A.v_[0], -A.v_[1], -A.v_[2], -A.v_[3], -A.v_[4], -A.v_[5], -A.v_[6], -A.v_[7], -A.v_[8] ); 00410 } 00411 00412 //! \return A+B 00413 inline const Matrix3D operator+( const Matrix3D& A, const Matrix3D& B ) 00414 { 00415 Matrix3D tmp( A ); 00416 tmp += B; 00417 return tmp; 00418 } 00419 00420 inline Matrix3D operator+( const Matrix3D& A, const SymMatrix3D& B ) 00421 { 00422 return Matrix3D( A( 0, 0 ) + B[SymMatrix3D::T00], A( 0, 1 ) + B[SymMatrix3D::T01], A( 0, 2 ) + B[SymMatrix3D::T02], 00423 A( 1, 0 ) + B[SymMatrix3D::T10], A( 1, 1 ) + B[SymMatrix3D::T11], A( 1, 2 ) + B[SymMatrix3D::T12], 00424 A( 2, 0 ) + B[SymMatrix3D::T20], A( 2, 1 ) + B[SymMatrix3D::T21], 00425 A( 2, 2 ) + B[SymMatrix3D::T22] ); 00426 } 00427 inline Matrix3D operator+( const SymMatrix3D& B, const Matrix3D& A ) 00428 { 00429 return A + B; 00430 } 00431 00432 //! \return A-B 00433 inline const Matrix3D operator-( const Matrix3D& A, const Matrix3D& B ) 00434 { 00435 Matrix3D tmp( A ); 00436 tmp -= B; 00437 return tmp; 00438 } 00439 00440 inline Matrix3D operator-( const Matrix3D& A, const SymMatrix3D& B ) 00441 { 00442 return Matrix3D( A( 0, 0 ) - B[SymMatrix3D::T00], A( 0, 1 ) - B[SymMatrix3D::T01], A( 0, 2 ) - B[SymMatrix3D::T02], 00443 A( 1, 0 ) - B[SymMatrix3D::T10], A( 1, 1 ) - B[SymMatrix3D::T11], A( 1, 2 ) - B[SymMatrix3D::T12], 00444 A( 2, 0 ) - B[SymMatrix3D::T20], A( 2, 1 ) - B[SymMatrix3D::T21], 00445 A( 2, 2 ) - B[SymMatrix3D::T22] ); 00446 } 00447 inline Matrix3D operator-( const SymMatrix3D& B, const Matrix3D& A ) 00448 { 00449 return Matrix3D( B[SymMatrix3D::T00] - A( 0, 0 ), B[SymMatrix3D::T01] - A( 0, 1 ), B[SymMatrix3D::T02] - A( 0, 2 ), 00450 B[SymMatrix3D::T10] - A( 1, 0 ), B[SymMatrix3D::T11] - A( 1, 1 ), B[SymMatrix3D::T12] - A( 1, 2 ), 00451 B[SymMatrix3D::T20] - A( 2, 0 ), B[SymMatrix3D::T21] - A( 2, 1 ), 00452 B[SymMatrix3D::T22] - A( 2, 2 ) ); 00453 } 00454 00455 inline Matrix3D& Matrix3D::equal_mult_elem( const Matrix3D& A ) 00456 { 00457 v_[0] *= A.v_[0]; 00458 v_[1] *= A.v_[1]; 00459 v_[2] *= A.v_[2]; 00460 v_[3] *= A.v_[3]; 00461 v_[4] *= A.v_[4]; 00462 v_[5] *= A.v_[5]; 00463 v_[6] *= A.v_[6]; 00464 v_[7] *= A.v_[7]; 00465 v_[8] *= A.v_[8]; 00466 return *this; 00467 } 00468 00469 //! Multiplies entry by entry. This is NOT a matrix multiplication. 00470 inline const Matrix3D mult_element( const Matrix3D& A, const Matrix3D& B ) 00471 { 00472 Matrix3D tmp( A ); 00473 tmp.equal_mult_elem( B ); 00474 return tmp; 00475 } 00476 00477 //! Return the square of the Frobenius norm of A, i.e. sum (diag (A' * A)) 00478 inline double Frobenius_2( const Matrix3D& A ) 00479 { 00480 return A.v_[0] * A.v_[0] + A.v_[1] * A.v_[1] + A.v_[2] * A.v_[2] + A.v_[3] * A.v_[3] + A.v_[4] * A.v_[4] + 00481 A.v_[5] * A.v_[5] + A.v_[6] * A.v_[6] + A.v_[7] * A.v_[7] + A.v_[8] * A.v_[8]; 00482 } 00483 00484 inline Matrix3D& Matrix3D::transpose() 00485 { 00486 double t; 00487 t = v_[1]; 00488 v_[1] = v_[3]; 00489 v_[3] = t; 00490 t = v_[2]; 00491 v_[2] = v_[6]; 00492 v_[6] = t; 00493 t = v_[5]; 00494 v_[5] = v_[7]; 00495 v_[7] = t; 00496 return *this; 00497 } 00498 00499 inline Matrix3D transpose( const Matrix3D& A ) 00500 { 00501 Matrix3D S; 00502 // size_t i; 00503 // for (i=0; i<3; ++i) { 00504 // S[size_t(0)][i] = A[i][0]; 00505 // S[size_t(1)][i] = A[i][1]; 00506 // S[size_t(2)][i] = A[i][2]; 00507 // } 00508 S.v_[0] = A.v_[0]; 00509 S.v_[1] = A.v_[3]; 00510 S.v_[2] = A.v_[6]; 00511 S.v_[3] = A.v_[1]; 00512 S.v_[4] = A.v_[4]; 00513 S.v_[5] = A.v_[7]; 00514 S.v_[6] = A.v_[2]; 00515 S.v_[7] = A.v_[5]; 00516 S.v_[8] = A.v_[8]; 00517 00518 return S; 00519 } 00520 00521 inline void Matrix3D::operator+=( const Matrix3D& rhs ) 00522 { 00523 v_[0] += rhs.v_[0]; 00524 v_[1] += rhs.v_[1]; 00525 v_[2] += rhs.v_[2]; 00526 v_[3] += rhs.v_[3]; 00527 v_[4] += rhs.v_[4]; 00528 v_[5] += rhs.v_[5]; 00529 v_[6] += rhs.v_[6]; 00530 v_[7] += rhs.v_[7]; 00531 v_[8] += rhs.v_[8]; 00532 } 00533 00534 inline void Matrix3D::operator+=( const SymMatrix3D& rhs ) 00535 { 00536 v_[0] += rhs[0]; 00537 v_[1] += rhs[1]; 00538 v_[2] += rhs[2]; 00539 v_[3] += rhs[1]; 00540 v_[4] += rhs[3]; 00541 v_[5] += rhs[4]; 00542 v_[6] += rhs[2]; 00543 v_[7] += rhs[4]; 00544 v_[8] += rhs[5]; 00545 } 00546 00547 inline void Matrix3D::operator-=( const Matrix3D& rhs ) 00548 { 00549 v_[0] -= rhs.v_[0]; 00550 v_[1] -= rhs.v_[1]; 00551 v_[2] -= rhs.v_[2]; 00552 v_[3] -= rhs.v_[3]; 00553 v_[4] -= rhs.v_[4]; 00554 v_[5] -= rhs.v_[5]; 00555 v_[6] -= rhs.v_[6]; 00556 v_[7] -= rhs.v_[7]; 00557 v_[8] -= rhs.v_[8]; 00558 } 00559 00560 inline void Matrix3D::operator-=( const SymMatrix3D& rhs ) 00561 { 00562 v_[0] -= rhs[0]; 00563 v_[1] -= rhs[1]; 00564 v_[2] -= rhs[2]; 00565 v_[3] -= rhs[1]; 00566 v_[4] -= rhs[3]; 00567 v_[5] -= rhs[4]; 00568 v_[6] -= rhs[2]; 00569 v_[7] -= rhs[4]; 00570 v_[8] -= rhs[5]; 00571 } 00572 00573 //! multiplies each entry by the scalar s 00574 inline void Matrix3D::operator*=( double s ) 00575 { 00576 v_[0] *= s; 00577 v_[1] *= s; 00578 v_[2] *= s; 00579 v_[3] *= s; 00580 v_[4] *= s; 00581 v_[5] *= s; 00582 v_[6] *= s; 00583 v_[7] *= s; 00584 v_[8] *= s; 00585 } 00586 00587 //! \f$ += B^T \f$ 00588 inline Matrix3D& Matrix3D::plus_transpose_equal( const Matrix3D& b ) 00589 { 00590 if( &b == this ) 00591 { 00592 v_[0] *= 2.0; 00593 v_[1] += v_[3]; 00594 v_[2] += v_[6]; 00595 v_[3] = v_[1]; 00596 v_[4] *= 2.0; 00597 v_[5] += v_[7]; 00598 v_[6] = v_[2]; 00599 v_[7] = v_[5]; 00600 v_[8] *= 2.0; 00601 } 00602 else 00603 { 00604 v_[0] += b.v_[0]; 00605 v_[1] += b.v_[3]; 00606 v_[2] += b.v_[6]; 00607 00608 v_[3] += b.v_[1]; 00609 v_[4] += b.v_[4]; 00610 v_[5] += b.v_[7]; 00611 00612 v_[6] += b.v_[2]; 00613 v_[7] += b.v_[5]; 00614 v_[8] += b.v_[8]; 00615 } 00616 return *this; 00617 } 00618 00619 //! \f$ + B^T \f$ 00620 inline Matrix3D plus_transpose( const Matrix3D& A, const Matrix3D& B ) 00621 { 00622 Matrix3D tmp( A ); 00623 tmp.plus_transpose_equal( B ); 00624 return tmp; 00625 } 00626 00627 //! Computes \f$ A = v_1 v_2^T \f$ 00628 inline Matrix3D& Matrix3D::outer_product( const Vector3D& v1, const Vector3D& v2 ) 00629 { 00630 // remember, matrix entries are v_[0] to v_[8]. 00631 00632 // diagonal 00633 v_[0] = v1[0] * v2[0]; 00634 v_[4] = v1[1] * v2[1]; 00635 v_[8] = v1[2] * v2[2]; 00636 00637 // upper triangular part 00638 v_[1] = v1[0] * v2[1]; 00639 v_[2] = v1[0] * v2[2]; 00640 v_[5] = v1[1] * v2[2]; 00641 00642 // lower triangular part 00643 v_[3] = v2[0] * v1[1]; 00644 v_[6] = v2[0] * v1[2]; 00645 v_[7] = v2[1] * v1[2]; 00646 00647 return *this; 00648 } 00649 00650 inline void Matrix3D::fill_lower_triangle() 00651 { 00652 v_[3] = v_[1]; 00653 v_[6] = v_[2]; 00654 v_[7] = v_[5]; 00655 } 00656 00657 //! \return A*B 00658 inline const Matrix3D operator*( const Matrix3D& A, const Matrix3D& B ) 00659 { 00660 Matrix3D tmp; 00661 tmp.assign_product( A, B ); 00662 return tmp; 00663 } 00664 00665 inline const Matrix3D operator*( const Matrix3D& A, const SymMatrix3D& B ) 00666 { 00667 return Matrix3D( 00668 A( 0, 0 ) * B[0] + A( 0, 1 ) * B[1] + A( 0, 2 ) * B[2], A( 0, 0 ) * B[1] + A( 0, 1 ) * B[3] + A( 0, 2 ) * B[4], 00669 A( 0, 0 ) * B[2] + A( 0, 1 ) * B[4] + A( 0, 2 ) * B[5], 00670 00671 A( 1, 0 ) * B[0] + A( 1, 1 ) * B[1] + A( 1, 2 ) * B[2], A( 1, 0 ) * B[1] + A( 1, 1 ) * B[3] + A( 1, 2 ) * B[4], 00672 A( 1, 0 ) * B[2] + A( 1, 1 ) * B[4] + A( 1, 2 ) * B[5], 00673 00674 A( 2, 0 ) * B[0] + A( 2, 1 ) * B[1] + A( 2, 2 ) * B[2], A( 2, 0 ) * B[1] + A( 2, 1 ) * B[3] + A( 2, 2 ) * B[4], 00675 A( 2, 0 ) * B[2] + A( 2, 1 ) * B[4] + A( 2, 2 ) * B[5] ); 00676 } 00677 00678 inline const Matrix3D operator*( const SymMatrix3D& B, const Matrix3D& A ) 00679 { 00680 return Matrix3D( 00681 A( 0, 0 ) * B[0] + A( 1, 0 ) * B[1] + A( 2, 0 ) * B[2], A( 0, 1 ) * B[0] + A( 1, 1 ) * B[1] + A( 2, 1 ) * B[2], 00682 A( 0, 2 ) * B[0] + A( 1, 2 ) * B[1] + A( 2, 2 ) * B[2], 00683 00684 A( 0, 0 ) * B[1] + A( 1, 0 ) * B[3] + A( 2, 0 ) * B[4], A( 0, 1 ) * B[1] + A( 1, 1 ) * B[3] + A( 2, 1 ) * B[4], 00685 A( 0, 2 ) * B[1] + A( 1, 2 ) * B[3] + A( 2, 2 ) * B[4], 00686 00687 A( 0, 0 ) * B[2] + A( 1, 0 ) * B[4] + A( 2, 0 ) * B[5], A( 0, 1 ) * B[2] + A( 1, 1 ) * B[4] + A( 2, 1 ) * B[5], 00688 A( 0, 2 ) * B[2] + A( 1, 2 ) * B[4] + A( 2, 2 ) * B[5] ); 00689 } 00690 00691 inline const Matrix3D operator*( const SymMatrix3D& a, const SymMatrix3D& b ) 00692 { 00693 return Matrix3D( a[0] * b[0] + a[1] * b[1] + a[2] * b[2], a[0] * b[1] + a[1] * b[3] + a[2] * b[4], 00694 a[0] * b[2] + a[1] * b[4] + a[2] * b[5], 00695 00696 a[1] * b[0] + a[3] * b[1] + a[4] * b[2], a[1] * b[1] + a[3] * b[3] + a[4] * b[4], 00697 a[1] * b[2] + a[3] * b[4] + a[4] * b[5], 00698 00699 a[2] * b[0] + a[4] * b[1] + a[5] * b[2], a[2] * b[1] + a[4] * b[3] + a[5] * b[4], 00700 a[2] * b[2] + a[4] * b[4] + a[5] * b[5] ); 00701 } 00702 00703 //! multiplies each entry by the scalar s 00704 inline const Matrix3D Matrix3D::operator*( double s ) const 00705 { 00706 Matrix3D temp( *this ); 00707 temp *= s; 00708 return temp; 00709 } 00710 //! friend function to allow for commutatative property of 00711 //! scalar mulitplication. 00712 inline const Matrix3D operator*( double s, const Matrix3D& A ) 00713 { 00714 return ( A.operator*( s ) ); 00715 } 00716 00717 inline Matrix3D& Matrix3D::assign_product( const Matrix3D& A, const Matrix3D& B ) 00718 { 00719 v_[0] = A.v_[0] * B.v_[0] + A.v_[1] * B.v_[3] + A.v_[2] * B.v_[6]; 00720 v_[1] = A.v_[0] * B.v_[1] + A.v_[1] * B.v_[4] + A.v_[2] * B.v_[7]; 00721 v_[2] = A.v_[0] * B.v_[2] + A.v_[1] * B.v_[5] + A.v_[2] * B.v_[8]; 00722 v_[3] = A.v_[3] * B.v_[0] + A.v_[4] * B.v_[3] + A.v_[5] * B.v_[6]; 00723 v_[4] = A.v_[3] * B.v_[1] + A.v_[4] * B.v_[4] + A.v_[5] * B.v_[7]; 00724 v_[5] = A.v_[3] * B.v_[2] + A.v_[4] * B.v_[5] + A.v_[5] * B.v_[8]; 00725 v_[6] = A.v_[6] * B.v_[0] + A.v_[7] * B.v_[3] + A.v_[8] * B.v_[6]; 00726 v_[7] = A.v_[6] * B.v_[1] + A.v_[7] * B.v_[4] + A.v_[8] * B.v_[7]; 00727 v_[8] = A.v_[6] * B.v_[2] + A.v_[7] * B.v_[5] + A.v_[8] * B.v_[8]; 00728 return *this; 00729 } 00730 00731 //! \f$ C = A \times B \f$ 00732 inline void matmult( Matrix3D& C, const Matrix3D& A, const Matrix3D& B ) 00733 { 00734 C.assign_product( A, B ); 00735 } 00736 00737 /*! \brief Computes \f$ A v \f$ . */ 00738 inline const Vector3D operator*( const Matrix3D& A, const Vector3D& x ) 00739 { 00740 Vector3D tmp; 00741 eqAx( tmp, A, x ); 00742 return tmp; 00743 } 00744 00745 /*! \brief Computes \f$ v^T A \f$ . 00746 00747 This function implicitly considers the transpose of vector x times 00748 the matrix A and it is implicit that the returned vector must be 00749 transposed. */ 00750 inline const Vector3D operator*( const Vector3D& x, const Matrix3D& A ) 00751 { 00752 Vector3D tmp; 00753 eqTransAx( tmp, A, x ); 00754 return tmp; 00755 } 00756 00757 inline void eqAx( Vector3D& v, const Matrix3D& A, const Vector3D& x ) 00758 { 00759 v.mCoords[0] = A.v_[0] * x[0] + A.v_[1] * x.mCoords[1] + A.v_[2] * x.mCoords[2]; 00760 v.mCoords[1] = A.v_[3] * x[0] + A.v_[4] * x.mCoords[1] + A.v_[5] * x.mCoords[2]; 00761 v.mCoords[2] = A.v_[6] * x[0] + A.v_[7] * x.mCoords[1] + A.v_[8] * x.mCoords[2]; 00762 } 00763 00764 inline void plusEqAx( Vector3D& v, const Matrix3D& A, const Vector3D& x ) 00765 { 00766 v.mCoords[0] += A.v_[0] * x[0] + A.v_[1] * x.mCoords[1] + A.v_[2] * x.mCoords[2]; 00767 v.mCoords[1] += A.v_[3] * x[0] + A.v_[4] * x.mCoords[1] + A.v_[5] * x.mCoords[2]; 00768 v.mCoords[2] += A.v_[6] * x[0] + A.v_[7] * x.mCoords[1] + A.v_[8] * x.mCoords[2]; 00769 } 00770 00771 inline void eqTransAx( Vector3D& v, const Matrix3D& A, const Vector3D& x ) 00772 { 00773 v.mCoords[0] = A.v_[0] * x.mCoords[0] + A.v_[3] * x.mCoords[1] + A.v_[6] * x.mCoords[2]; 00774 v.mCoords[1] = A.v_[1] * x.mCoords[0] + A.v_[4] * x.mCoords[1] + A.v_[7] * x.mCoords[2]; 00775 v.mCoords[2] = A.v_[2] * x.mCoords[0] + A.v_[5] * x.mCoords[1] + A.v_[8] * x.mCoords[2]; 00776 } 00777 00778 inline void plusEqTransAx( Vector3D& v, const Matrix3D& A, const Vector3D& x ) 00779 { 00780 v.mCoords[0] += A.v_[0] * x.mCoords[0] + A.v_[3] * x.mCoords[1] + A.v_[6] * x.mCoords[2]; 00781 v.mCoords[1] += A.v_[1] * x.mCoords[0] + A.v_[4] * x.mCoords[1] + A.v_[7] * x.mCoords[2]; 00782 v.mCoords[2] += A.v_[2] * x.mCoords[0] + A.v_[5] * x.mCoords[1] + A.v_[8] * x.mCoords[2]; 00783 } 00784 00785 inline void plusEqaA( Matrix3D& B, const double a, const Matrix3D& A ) 00786 { 00787 B.v_[0] += a * A.v_[0]; 00788 B.v_[1] += a * A.v_[1]; 00789 B.v_[2] += a * A.v_[2]; 00790 B.v_[3] += a * A.v_[3]; 00791 B.v_[4] += a * A.v_[4]; 00792 B.v_[5] += a * A.v_[5]; 00793 B.v_[6] += a * A.v_[6]; 00794 B.v_[7] += a * A.v_[7]; 00795 B.v_[8] += a * A.v_[8]; 00796 } 00797 00798 inline double det( const Matrix3D& A ) 00799 { 00800 return ( A.v_[0] * ( A.v_[4] * A.v_[8] - A.v_[7] * A.v_[5] ) - A.v_[1] * ( A.v_[3] * A.v_[8] - A.v_[6] * A.v_[5] ) + 00801 A.v_[2] * ( A.v_[3] * A.v_[7] - A.v_[6] * A.v_[4] ) ); 00802 } 00803 00804 inline void inv( Matrix3D& Ainv, const Matrix3D& A ) 00805 { 00806 double inv_detA = 1.0 / ( det( A ) ); 00807 // First row of Ainv 00808 Ainv.v_[0] = inv_detA * ( A.v_[4] * A.v_[8] - A.v_[5] * A.v_[7] ); 00809 Ainv.v_[1] = inv_detA * ( A.v_[2] * A.v_[7] - A.v_[8] * A.v_[1] ); 00810 Ainv.v_[2] = inv_detA * ( A.v_[1] * A.v_[5] - A.v_[4] * A.v_[2] ); 00811 // Second row of Ainv 00812 Ainv.v_[3] = inv_detA * ( A.v_[5] * A.v_[6] - A.v_[8] * A.v_[3] ); 00813 Ainv.v_[4] = inv_detA * ( A.v_[0] * A.v_[8] - A.v_[6] * A.v_[2] ); 00814 Ainv.v_[5] = inv_detA * ( A.v_[2] * A.v_[3] - A.v_[5] * A.v_[0] ); 00815 // Third row of Ainv 00816 Ainv.v_[6] = inv_detA * ( A.v_[3] * A.v_[7] - A.v_[6] * A.v_[4] ); 00817 Ainv.v_[7] = inv_detA * ( A.v_[1] * A.v_[6] - A.v_[7] * A.v_[0] ); 00818 Ainv.v_[8] = inv_detA * ( A.v_[0] * A.v_[4] - A.v_[3] * A.v_[1] ); 00819 } 00820 00821 inline void timesInvA( Matrix3D& B, const Matrix3D& A ) 00822 { 00823 00824 Matrix3D Ainv; 00825 inv( Ainv, A ); 00826 B = B * Ainv; 00827 } 00828 00829 inline void QR( Matrix3D& Q, Matrix3D& R, const Matrix3D& A ) 00830 { 00831 // Compute the QR factorization of A. This code uses the 00832 // Modified Gram-Schmidt method for computing the factorization. 00833 // The Householder version is more stable, but costs twice as many 00834 // floating point operations. 00835 00836 Q = A; 00837 00838 R[0][0] = sqrt( Q[0][0] * Q[0][0] + Q[1][0] * Q[1][0] + Q[2][0] * Q[2][0] ); 00839 double temp_dbl = 1.0 / R[0][0]; 00840 R[1][0] = 0.0L; 00841 R[2][0] = 0.0L; 00842 // Q[0][0] /= R[0][0]; 00843 // Q[1][0] /= R[0][0]; 00844 // Q[2][0] /= R[0][0]; 00845 Q[0][0] *= temp_dbl; 00846 Q[1][0] *= temp_dbl; 00847 Q[2][0] *= temp_dbl; 00848 00849 R[0][1] = Q[0][0] * Q[0][1] + Q[1][0] * Q[1][1] + Q[2][0] * Q[2][1]; 00850 Q[0][1] -= Q[0][0] * R[0][1]; 00851 Q[1][1] -= Q[1][0] * R[0][1]; 00852 Q[2][1] -= Q[2][0] * R[0][1]; 00853 00854 R[0][2] = Q[0][0] * Q[0][2] + Q[1][0] * Q[1][2] + Q[2][0] * Q[2][2]; 00855 Q[0][2] -= Q[0][0] * R[0][2]; 00856 Q[1][2] -= Q[1][0] * R[0][2]; 00857 Q[2][2] -= Q[2][0] * R[0][2]; 00858 00859 R[1][1] = sqrt( Q[0][1] * Q[0][1] + Q[1][1] * Q[1][1] + Q[2][1] * Q[2][1] ); 00860 temp_dbl = 1.0 / R[1][1]; 00861 R[2][1] = 0.0L; 00862 // Q[0][1] /= R[1][1]; 00863 // Q[1][1] /= R[1][1]; 00864 // Q[2][1] /= R[1][1]; 00865 Q[0][1] *= temp_dbl; 00866 Q[1][1] *= temp_dbl; 00867 Q[2][1] *= temp_dbl; 00868 00869 R[1][2] = Q[0][1] * Q[0][2] + Q[1][1] * Q[1][2] + Q[2][1] * Q[2][2]; 00870 Q[0][2] -= Q[0][1] * R[1][2]; 00871 Q[1][2] -= Q[1][1] * R[1][2]; 00872 Q[2][2] -= Q[2][1] * R[1][2]; 00873 00874 R[2][2] = sqrt( Q[0][2] * Q[0][2] + Q[1][2] * Q[1][2] + Q[2][2] * Q[2][2] ); 00875 temp_dbl = 1.0 / R[2][2]; 00876 00877 // Q[0][2] /= R[2][2]; 00878 // Q[1][2] /= R[2][2]; 00879 // Q[2][2] /= R[2][2]; 00880 Q[0][2] *= temp_dbl; 00881 Q[1][2] *= temp_dbl; 00882 Q[2][2] *= temp_dbl; 00883 00884 return; 00885 } 00886 00887 inline bool Matrix3D::positive_definite() const 00888 { 00889 // A = B + C 00890 // where 00891 // B = (A + transpose(A))/2 00892 // C = (A - transpose(A))/2 00893 // B is always a symmetric matrix and 00894 // A is positive definite iff B is positive definite. 00895 Matrix3D B( *this ); 00896 B.plus_transpose_equal( *this ); 00897 B *= 0.5; 00898 00899 // Sylvester's Criterion for positive definite symmetric matrix 00900 return ( B[0][0] > 0.0 ) && ( B.sub_det( 2, 2 ) > 0.0 ) && ( det( B ) > 0.0 ); 00901 } 00902 00903 } // namespace MBMesquite 00904 00905 #endif // Matrix3D_hpp