MOAB: Mesh Oriented datABase  (version 5.3.1)
Matrix3D.hpp
Go to the documentation of this file.
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
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines