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