MOAB: Mesh Oriented datABase  (version 5.4.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,
00915                                           unsigned c,
00916                                           const MsqMatrix< R, RC >& A,
00917                                           const MsqMatrix< RC, C >& B )
00918 {
00919     double tmp = A( r, 0 ) * B( 0, c );
00920     switch( RC )
00921     {
00922         default:
00923             for( unsigned k = 6; k < RC; ++k )
00924                 tmp += A( r, k ) * B( k, c );
00925         case 6:
00926             tmp += A( r, 5 ) * B( 5, c );
00927         case 5:
00928             tmp += A( r, 4 ) * B( 4, c );
00929         case 4:
00930             tmp += A( r, 3 ) * B( 3, c );
00931         case 3:
00932             tmp += A( r, 2 ) * B( 2, c );
00933         case 2:
00934             tmp += A( r, 1 ) * B( 1, c );
00935         case 1:;
00936     }
00937     return tmp;
00938 }
00939 
00940 template < unsigned R, unsigned RC, unsigned C >
00941 inline MsqMatrix< R, C > operator*( const MsqMatrix< R, RC >& A, const MsqMatrix< RC, C >& B )
00942 {
00943     // MsqMatrix<R,C> result(0.0);
00944     // for (unsigned i = 0; i < R; ++i)
00945     //  for (unsigned j = 0; j < C; ++j)
00946     //    for (unsigned k = 0; k < RC; ++k)
00947     //      result(i,j) += A(i,k) * B(k,j);
00948 
00949     MsqMatrix< R, C > result;
00950     for( unsigned r = 0; r < R; ++r )
00951         for( unsigned c = 0; c < C; ++c )
00952             result( r, c ) = multiply_helper_result_val( r, c, A, B );
00953 
00954     return result;
00955 }
00956 
00957 template < unsigned R, unsigned C >
00958 inline MsqMatrix< R, C > operator/( const MsqMatrix< R, C >& m, double s )
00959 {
00960     MsqMatrix< R, C > tmp( m );
00961     tmp /= s;
00962     return tmp;
00963 }
00964 
00965 template < unsigned RC >
00966 inline double cofactor( const MsqMatrix< RC, RC >& m, unsigned r, unsigned c )
00967 {
00968     const double sign[] = { 1.0, -1.0 };
00969     return sign[( r + c ) % 2] * det( MsqMatrix< RC - 1, RC - 1 >( m, r, c ) );
00970 }
00971 
00972 template < unsigned RC >
00973 inline double det( const MsqMatrix< RC, RC >& m )
00974 {
00975     double result = 0.0;
00976     for( unsigned i = 0; i < RC; ++i )
00977         result += m( 0, i ) * cofactor< RC >( m, 0, i );
00978     return result;
00979 }
00980 
00981 // inline
00982 // double det( const MsqMatrix<1,1>& m )
00983 //  { return m(0,0); }
00984 
00985 inline double det( const MsqMatrix< 2, 2 >& m )
00986 {
00987     return m( 0, 0 ) * m( 1, 1 ) - m( 0, 1 ) * m( 1, 0 );
00988 }
00989 
00990 inline double det( const MsqMatrix< 3, 3 >& m )
00991 {
00992     return m( 0, 0 ) * ( m( 1, 1 ) * m( 2, 2 ) - m( 2, 1 ) * m( 1, 2 ) ) +
00993            m( 0, 1 ) * ( m( 2, 0 ) * m( 1, 2 ) - m( 1, 0 ) * m( 2, 2 ) ) +
00994            m( 0, 2 ) * ( m( 1, 0 ) * m( 2, 1 ) - m( 2, 0 ) * m( 1, 1 ) );
00995 }
00996 
00997 inline MsqMatrix< 2, 2 > adj( const MsqMatrix< 2, 2 >& m )
00998 {
00999     MsqMatrix< 2, 2 > result;
01000     result( 0, 0 ) = m( 1, 1 );
01001     result( 0, 1 ) = -m( 0, 1 );
01002     result( 1, 0 ) = -m( 1, 0 );
01003     result( 1, 1 ) = m( 0, 0 );
01004     return result;
01005 }
01006 
01007 inline MsqMatrix< 2, 2 > transpose_adj( const MsqMatrix< 2, 2 >& m )
01008 {
01009     MsqMatrix< 2, 2 > result;
01010     result( 0, 0 ) = m( 1, 1 );
01011     result( 1, 0 ) = -m( 0, 1 );
01012     result( 0, 1 ) = -m( 1, 0 );
01013     result( 1, 1 ) = m( 0, 0 );
01014     return result;
01015 }
01016 
01017 inline MsqMatrix< 3, 3 > adj( const MsqMatrix< 3, 3 >& m )
01018 {
01019     MsqMatrix< 3, 3 > result;
01020 
01021     result( 0, 0 ) = m( 1, 1 ) * m( 2, 2 ) - m( 1, 2 ) * m( 2, 1 );
01022     result( 0, 1 ) = m( 0, 2 ) * m( 2, 1 ) - m( 0, 1 ) * m( 2, 2 );
01023     result( 0, 2 ) = m( 0, 1 ) * m( 1, 2 ) - m( 0, 2 ) * m( 1, 1 );
01024 
01025     result( 1, 0 ) = m( 1, 2 ) * m( 2, 0 ) - m( 1, 0 ) * m( 2, 2 );
01026     result( 1, 1 ) = m( 0, 0 ) * m( 2, 2 ) - m( 0, 2 ) * m( 2, 0 );
01027     result( 1, 2 ) = m( 0, 2 ) * m( 1, 0 ) - m( 0, 0 ) * m( 1, 2 );
01028 
01029     result( 2, 0 ) = m( 1, 0 ) * m( 2, 1 ) - m( 1, 1 ) * m( 2, 0 );
01030     result( 2, 1 ) = m( 0, 1 ) * m( 2, 0 ) - m( 0, 0 ) * m( 2, 1 );
01031     result( 2, 2 ) = m( 0, 0 ) * m( 1, 1 ) - m( 0, 1 ) * m( 1, 0 );
01032 
01033     return result;
01034 }
01035 
01036 inline MsqMatrix< 3, 3 > transpose_adj( const MsqMatrix< 3, 3 >& m )
01037 {
01038     MsqMatrix< 3, 3 > result;
01039 
01040     result( 0, 0 ) = m( 1, 1 ) * m( 2, 2 ) - m( 1, 2 ) * m( 2, 1 );
01041     result( 0, 1 ) = m( 1, 2 ) * m( 2, 0 ) - m( 1, 0 ) * m( 2, 2 );
01042     result( 0, 2 ) = m( 1, 0 ) * m( 2, 1 ) - m( 1, 1 ) * m( 2, 0 );
01043 
01044     result( 1, 0 ) = m( 0, 2 ) * m( 2, 1 ) - m( 0, 1 ) * m( 2, 2 );
01045     result( 1, 1 ) = m( 0, 0 ) * m( 2, 2 ) - m( 0, 2 ) * m( 2, 0 );
01046     result( 1, 2 ) = m( 0, 1 ) * m( 2, 0 ) - m( 0, 0 ) * m( 2, 1 );
01047 
01048     result( 2, 0 ) = m( 0, 1 ) * m( 1, 2 ) - m( 0, 2 ) * m( 1, 1 );
01049     result( 2, 1 ) = m( 0, 2 ) * m( 1, 0 ) - m( 0, 0 ) * m( 1, 2 );
01050     result( 2, 2 ) = m( 0, 0 ) * m( 1, 1 ) - m( 0, 1 ) * m( 1, 0 );
01051 
01052     return result;
01053 }
01054 
01055 template < unsigned R, unsigned C >
01056 inline MsqMatrix< R, C > inverse( const MsqMatrix< R, C >& m )
01057 {
01058     return adj( m ) * ( 1.0 / det( m ) );
01059 }
01060 
01061 template < unsigned R, unsigned C >
01062 inline MsqMatrix< R, C > transpose( const MsqMatrix< C, R >& m )
01063 {
01064     MsqMatrix< R, C > result;
01065     for( unsigned r = 0; r < R; ++r )
01066         for( unsigned c = 0; c < C; ++c )
01067             result( r, c ) = m( c, r );
01068     return result;
01069 }
01070 /*
01071 template <unsigned R> inline
01072 const MsqMatrix<R,1>& transpose( const MsqMatrix<1,R>& m )
01073   { return *reinterpret_cast<const MsqMatrix<R,1>*>(&m); }
01074 
01075 template <unsigned C> inline
01076 const MsqMatrix<1,C>& transpose( const MsqMatrix<C,1>& m )
01077   { return *reinterpret_cast<const MsqMatrix<1,C>*>(&m); }
01078 */
01079 template < unsigned RC >
01080 inline double trace( const MsqMatrix< RC, RC >& m )
01081 {
01082     double result = m( 0, 0 );
01083     for( unsigned i = 1; i < RC; ++i )
01084         result += m( i, i );
01085     return result;
01086 }
01087 
01088 template < unsigned R, unsigned C >
01089 inline double sqr_Frobenius( const MsqMatrix< R, C >& m )
01090 {
01091     double result = *m.data() * *m.data();
01092     for( unsigned i = 1; i < R * C; ++i )
01093         result += m.data()[i] * m.data()[i];
01094     return result;
01095 }
01096 
01097 template < unsigned R, unsigned C >
01098 inline double Frobenius( const MsqMatrix< R, C >& m )
01099 {
01100     return std::sqrt( sqr_Frobenius< R, C >( m ) );
01101 }
01102 
01103 template < unsigned R, unsigned C >
01104 inline bool operator==( const MsqMatrix< R, C >& A, const MsqMatrix< R, C >& B )
01105 {
01106     for( unsigned i = 0; i < R * C; ++i )
01107         if( A.data()[i] != B.data()[i] ) return false;
01108     return true;
01109 }
01110 
01111 template < unsigned R, unsigned C >
01112 inline bool operator!=( const MsqMatrix< R, C >& A, const MsqMatrix< R, C >& B )
01113 {
01114     return !( A == B );
01115 }
01116 
01117 template < unsigned R, unsigned C >
01118 inline std::ostream& operator<<( std::ostream& str, const MsqMatrix< R, C >& m )
01119 {
01120     str << m.data()[0];
01121     for( unsigned i = 1; i < R * C; ++i )
01122         str << ' ' << m.data()[i];
01123     return str;
01124 }
01125 
01126 template < unsigned R, unsigned C >
01127 inline std::istream& operator>>( std::istream& str, MsqMatrix< R, C >& m )
01128 {
01129     for( unsigned i = 0; i < R * C; ++i )
01130         str >> m.data()[i];
01131     return str;
01132 }
01133 
01134 template < unsigned R >
01135 inline double sqr_length( const MsqMatrix< R, 1 >& v )
01136 {
01137     return sqr_Frobenius( v );
01138 }
01139 
01140 template < unsigned C >
01141 inline double sqr_length( const MsqMatrix< 1, C >& v )
01142 {
01143     return sqr_Frobenius( v );
01144 }
01145 
01146 template < unsigned R >
01147 inline double length( const MsqMatrix< R, 1 >& v )
01148 {
01149     return Frobenius( v );
01150 }
01151 
01152 template < unsigned C >
01153 inline double length( const MsqMatrix< 1, C >& v )
01154 {
01155     return Frobenius( v );
01156 }
01157 
01158 template < unsigned R, unsigned C >
01159 inline double inner_product( const MsqMatrix< R, C >& m1, const MsqMatrix< R, C >& m2 )
01160 {
01161     double result = 0.0;
01162     for( unsigned r = 0; r < R; ++r )
01163         for( unsigned c = 0; c < C; ++c )
01164             result += m1( r, c ) * m2( r, c );
01165     return result;
01166 }
01167 
01168 template < unsigned R, unsigned C >
01169 inline MsqMatrix< R, C > outer( const MsqMatrix< R, 1 >& v1, const MsqMatrix< C, 1 >& v2 )
01170 {
01171     MsqMatrix< R, C > result;
01172     for( unsigned r = 0; r < R; ++r )
01173         for( unsigned c = 0; c < C; ++c )
01174             result( r, c ) = v1( r, 0 ) * v2( c, 0 );
01175     return result;
01176 }
01177 
01178 template < unsigned R, unsigned C >
01179 inline MsqMatrix< R, C > outer( const MsqMatrix< 1, R >& v1, const MsqMatrix< 1, C >& v2 )
01180 {
01181     MsqMatrix< R, C > result;
01182     for( unsigned r = 0; r < R; ++r )
01183         for( unsigned c = 0; c < C; ++c )
01184             result( r, c ) = v1( 0, r ) * v2( 0, c );
01185     return result;
01186 }
01187 
01188 inline double vector_product( const MsqMatrix< 2, 1 >& v1, const MsqMatrix< 2, 1 >& v2 )
01189 {
01190     return v1( 0, 0 ) * v2( 1, 0 ) - v1( 1, 0 ) * v2( 0, 0 );
01191 }
01192 
01193 inline double vector_product( const MsqMatrix< 1, 2 >& v1, const MsqMatrix< 1, 2 >& v2 )
01194 {
01195     return v1( 0, 0 ) * v2( 0, 1 ) - v1( 0, 1 ) * v2( 0, 0 );
01196 }
01197 
01198 inline MsqMatrix< 3, 1 > vector_product( const MsqMatrix< 3, 1 >& a, const MsqMatrix< 3, 1 >& b )
01199 {
01200     MsqMatrix< 3, 1 > result;
01201     result( 0, 0 ) = a( 1, 0 ) * b( 2, 0 ) - a( 2, 0 ) * b( 1, 0 );
01202     result( 1, 0 ) = a( 2, 0 ) * b( 0, 0 ) - a( 0, 0 ) * b( 2, 0 );
01203     result( 2, 0 ) = a( 0, 0 ) * b( 1, 0 ) - a( 1, 0 ) * b( 0, 0 );
01204     return result;
01205 }
01206 
01207 inline MsqMatrix< 1, 3 > vector_product( const MsqMatrix< 1, 3 >& a, const MsqMatrix< 1, 3 >& b )
01208 {
01209     MsqMatrix< 1, 3 > result;
01210     result( 0, 0 ) = a( 0, 1 ) * b( 0, 2 ) - a( 0, 2 ) * b( 0, 1 );
01211     result( 0, 1 ) = a( 0, 2 ) * b( 0, 0 ) - a( 0, 0 ) * b( 0, 2 );
01212     result( 0, 2 ) = a( 0, 0 ) * b( 0, 1 ) - a( 0, 1 ) * b( 0, 0 );
01213     return result;
01214 }
01215 
01216 template < unsigned R, unsigned C >
01217 inline double operator%( const MsqMatrix< R, C >& v1, const MsqMatrix< R, C >& v2 )
01218 {
01219     return inner_product( v1, v2 );
01220 }
01221 
01222 inline double operator*( const MsqMatrix< 2, 1 >& v1, const MsqMatrix< 2, 1 >& v2 )
01223 {
01224     return vector_product( v1, v2 );
01225 }
01226 
01227 inline double operator*( const MsqMatrix< 1, 2 >& v1, const MsqMatrix< 1, 2 >& v2 )
01228 {
01229     return vector_product( v1, v2 );
01230 }
01231 
01232 inline MsqMatrix< 3, 1 > operator*( const MsqMatrix< 3, 1 >& v1, const MsqMatrix< 3, 1 >& v2 )
01233 {
01234     return vector_product( v1, v2 );
01235 }
01236 
01237 inline MsqMatrix< 1, 3 > operator*( const MsqMatrix< 1, 3 >& v1, const MsqMatrix< 1, 3 >& v2 )
01238 {
01239     return vector_product( v1, v2 );
01240 }
01241 
01242 /** Compute QR factorization of A */
01243 inline void QR( const MsqMatrix< 3, 3 >& A, MsqMatrix< 3, 3 >& Q, MsqMatrix< 3, 3 >& R )
01244 {
01245     Q = A;
01246 
01247     R( 0, 0 )       = sqrt( Q( 0, 0 ) * Q( 0, 0 ) + Q( 1, 0 ) * Q( 1, 0 ) + Q( 2, 0 ) * Q( 2, 0 ) );
01248     double temp_dbl = 1.0 / R( 0, 0 );
01249     R( 1, 0 )       = 0.0;
01250     R( 2, 0 )       = 0.0;
01251     Q( 0, 0 ) *= temp_dbl;
01252     Q( 1, 0 ) *= temp_dbl;
01253     Q( 2, 0 ) *= temp_dbl;
01254 
01255     R( 0, 1 ) = Q( 0, 0 ) * Q( 0, 1 ) + Q( 1, 0 ) * Q( 1, 1 ) + Q( 2, 0 ) * Q( 2, 1 );
01256     Q( 0, 1 ) -= Q( 0, 0 ) * R( 0, 1 );
01257     Q( 1, 1 ) -= Q( 1, 0 ) * R( 0, 1 );
01258     Q( 2, 1 ) -= Q( 2, 0 ) * R( 0, 1 );
01259 
01260     R( 0, 2 ) = Q( 0, 0 ) * Q( 0, 2 ) + Q( 1, 0 ) * Q( 1, 2 ) + Q( 2, 0 ) * Q( 2, 2 );
01261     Q( 0, 2 ) -= Q( 0, 0 ) * R( 0, 2 );
01262     Q( 1, 2 ) -= Q( 1, 0 ) * R( 0, 2 );
01263     Q( 2, 2 ) -= Q( 2, 0 ) * R( 0, 2 );
01264 
01265     R( 1, 1 ) = sqrt( Q( 0, 1 ) * Q( 0, 1 ) + Q( 1, 1 ) * Q( 1, 1 ) + Q( 2, 1 ) * Q( 2, 1 ) );
01266     temp_dbl  = 1.0 / R( 1, 1 );
01267     R( 2, 1 ) = 0.0;
01268     Q( 0, 1 ) *= temp_dbl;
01269     Q( 1, 1 ) *= temp_dbl;
01270     Q( 2, 1 ) *= temp_dbl;
01271 
01272     R( 1, 2 ) = Q( 0, 1 ) * Q( 0, 2 ) + Q( 1, 1 ) * Q( 1, 2 ) + Q( 2, 1 ) * Q( 2, 2 );
01273     Q( 0, 2 ) -= Q( 0, 1 ) * R( 1, 2 );
01274     Q( 1, 2 ) -= Q( 1, 1 ) * R( 1, 2 );
01275     Q( 2, 2 ) -= Q( 2, 1 ) * R( 1, 2 );
01276 
01277     R( 2, 2 ) = sqrt( Q( 0, 2 ) * Q( 0, 2 ) + Q( 1, 2 ) * Q( 1, 2 ) + Q( 2, 2 ) * Q( 2, 2 ) );
01278     temp_dbl  = 1.0 / R( 2, 2 );
01279     Q( 0, 2 ) *= temp_dbl;
01280     Q( 1, 2 ) *= temp_dbl;
01281     Q( 2, 2 ) *= temp_dbl;
01282 }
01283 
01284 /** Compute QR factorization of A */
01285 inline void QR( const MsqMatrix< 2, 2 >& A, MsqMatrix< 2, 2 >& Q, MsqMatrix< 2, 2 >& R )
01286 {
01287     R( 0, 0 )          = std::sqrt( A( 0, 0 ) * A( 0, 0 ) + A( 1, 0 ) * A( 1, 0 ) );
01288     const double r0inv = 1.0 / R( 0, 0 );
01289     Q( 0, 0 ) = Q( 1, 1 ) = A( 0, 0 ) * r0inv;
01290     Q( 1, 0 )             = A( 1, 0 ) * r0inv;
01291     Q( 0, 1 )             = -Q( 1, 0 );
01292     R( 0, 1 )             = r0inv * ( A( 0, 0 ) * A( 0, 1 ) + A( 1, 0 ) * A( 1, 1 ) );
01293     R( 1, 1 )             = r0inv * ( A( 0, 0 ) * A( 1, 1 ) - A( 0, 1 ) * A( 1, 0 ) );
01294     R( 1, 0 )             = 0.0;
01295 }
01296 
01297 }  // namespace MBMesquite
01298 
01299 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines