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