MatrixOps.h Source File

00001 /************************************************************** ggt-head beg
00002  *
00003  * GGT: Generic Graphics Toolkit
00004  *
00005  * Original Authors:
00006  *   Allen Bierbaum
00007  *
00008  * -----------------------------------------------------------------
00009  * File:          $RCSfile: MatrixOps.h,v $
00010  * Date modified: $Date: 2003/03/17 18:34:36 $
00011  * Version:       $Revision: 1.30 $
00012  * -----------------------------------------------------------------
00013  *
00014  *********************************************************** ggt-head end */
00015 /*************************************************************** ggt-cpr beg
00016 *
00017 * GGT: The Generic Graphics Toolkit
00018 * Copyright (C) 2001,2002 Allen Bierbaum
00019 *
00020 * This library is free software; you can redistribute it and/or
00021 * modify it under the terms of the GNU Lesser General Public
00022 * License as published by the Free Software Foundation; either
00023 * version 2.1 of the License, or (at your option) any later version.
00024 *
00025 * This library is distributed in the hope that it will be useful,
00026 * but WITHOUT ANY WARRANTY; without even the implied warranty of
00027 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
00028 * Lesser General Public License for more details.
00029 *
00030 * You should have received a copy of the GNU Lesser General Public
00031 * License along with this library; if not, write to the Free Software
00032 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
00033 *
00034  ************************************************************ ggt-cpr end */
00035 #ifndef _GMTL_MATRIXOPS_H_
00036 #define _GMTL_MATRIXOPS_H_
00037 
00038 #include <iostream>         // for std::cerr
00039 #include <algorithm>        // needed for std::swap
00040 #include <gmtl/Matrix.h>
00041 #include <gmtl/Math.h>
00042 #include <gmtl/Vec.h>
00043 
00044 namespace gmtl
00045 {
00054    template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
00055    inline Matrix<DATA_TYPE, ROWS, COLS>& identity( Matrix<DATA_TYPE, ROWS, COLS>& result )
00056    {
00057       if(result.mState != Matrix<DATA_TYPE, ROWS, COLS>::IDENTITY)   // if not already ident
00058       {
00059          // TODO: mp
00060          for (unsigned int r = 0; r < ROWS; ++r)
00061          for (unsigned int c = 0; c < COLS; ++c)
00062             result( r, c ) = (DATA_TYPE)0.0;
00063 
00064          // TODO: mp
00065          for (unsigned int x = 0; x < Math::Min( COLS, ROWS ); ++x)
00066             result( x, x ) = (DATA_TYPE)1.0;
00067 
00068 //         result.mState = Matrix<DATA_TYPE, ROWS, COLS>::IDENTITY;
00069          result.mState = Matrix<DATA_TYPE, ROWS, COLS>::FULL;
00070       }
00071 
00072       return result;
00073    }
00074 
00075 
00079    template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
00080    inline Matrix<DATA_TYPE, ROWS, COLS>& zero( Matrix<DATA_TYPE, ROWS, COLS>& result )
00081    {
00082       if (result.mState == Matrix<DATA_TYPE, ROWS, COLS>::IDENTITY)
00083       {
00084          for (unsigned int x = 0; x < Math::Min( ROWS, COLS ); ++x)
00085          {
00086             result( x, x ) = (DATA_TYPE)0;
00087          }
00088       }
00089       else
00090       {
00091          for (unsigned int x = 0; x < ROWS*COLS; ++x)
00092          {
00093             result.mData[x] = (DATA_TYPE)0;
00094          }
00095       }
00096       return result;
00097    }
00098 
00099 
00105    template <typename DATA_TYPE, unsigned ROWS, unsigned INTERNAL, unsigned COLS>
00106    inline Matrix<DATA_TYPE, ROWS, COLS>& mult( Matrix<DATA_TYPE, ROWS, COLS>& result,
00107                  const Matrix<DATA_TYPE, ROWS, INTERNAL>& lhs,
00108                  const Matrix<DATA_TYPE, INTERNAL, COLS>& rhs )
00109    {
00110       Matrix<DATA_TYPE, ROWS, COLS> ret_mat; // prevent aliasing
00111       zero( ret_mat );
00112 
00113       // p. 150 Numerical Analysis (second ed.)
00114       // if A is m x p, and B is p x n, then AB is m x n
00115       // (AB)ij  =  [k = 1 to p] (a)ik (b)kj     (where:  1 <= i <= m, 1 <= j <= n)
00116       for (unsigned int i = 0; i < ROWS; ++i)           // 1 <= i <= m
00117       for (unsigned int j = 0; j < COLS; ++j)           // 1 <= j <= n
00118       for (unsigned int k = 0; k < INTERNAL; ++k)       // [k = 1 to p]
00119          ret_mat( i, j ) += lhs( i, k ) * rhs( k, j );
00120 
00121       return result = ret_mat;
00122    }
00123 
00129    template <typename DATA_TYPE, unsigned ROWS, unsigned INTERNAL, unsigned COLS>
00130    inline Matrix<DATA_TYPE, ROWS, COLS> operator*( const Matrix<DATA_TYPE, ROWS, INTERNAL>& lhs,
00131                                                    const Matrix<DATA_TYPE, INTERNAL, COLS>& rhs )
00132    {
00133       Matrix<DATA_TYPE, ROWS, COLS> temporary;
00134       return mult( temporary, lhs, rhs );
00135    }
00136 
00142    template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
00143    inline Matrix<DATA_TYPE, ROWS, COLS>& sub( Matrix<DATA_TYPE, ROWS, COLS>& result,
00144                  const Matrix<DATA_TYPE, ROWS, COLS>& lhs,
00145                  const Matrix<DATA_TYPE, ROWS, COLS>& rhs )
00146    {
00147       // p. 150 Numerical Analysis (second ed.)
00148       // if A is m x n, and B is m x n, then AB is m x n
00149       // (A - B)ij  = (a)ij - (b)ij     (where:  1 <= i <= m, 1 <= j <= n)
00150       for (unsigned int i = 0; i < ROWS; ++i)           // 1 <= i <= m
00151       for (unsigned int j = 0; j < COLS; ++j)           // 1 <= j <= n
00152          result( i, j ) = lhs( i, j ) - rhs( i, j );
00153 
00154       return result;
00155    }
00156 
00162    template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
00163    inline Matrix<DATA_TYPE, ROWS, COLS>& add( Matrix<DATA_TYPE, ROWS, COLS>& result,
00164                  const Matrix<DATA_TYPE, ROWS, COLS>& lhs,
00165                  const Matrix<DATA_TYPE, ROWS, COLS>& rhs )
00166    {
00167       // p. 150 Numerical Analysis (second ed.)
00168       // if A is m x n, and B is m x n, then AB is m x n
00169       // (A - B)ij  = (a)ij + (b)ij     (where:  1 <= i <= m, 1 <= j <= n)
00170       for (unsigned int i = 0; i < ROWS; ++i)           // 1 <= i <= m
00171       for (unsigned int j = 0; j < COLS; ++j)           // 1 <= j <= n
00172          result( i, j ) = lhs( i, j ) + rhs( i, j );
00173 
00174       return result;
00175    }
00176 
00181    template <typename DATA_TYPE, unsigned SIZE>
00182    inline Matrix<DATA_TYPE, SIZE, SIZE>& postMult( Matrix<DATA_TYPE, SIZE, SIZE>& result,
00183                                                      const Matrix<DATA_TYPE, SIZE, SIZE>& operand )
00184    {
00185       return mult( result, result, operand );
00186    }
00187 
00192    template <typename DATA_TYPE, unsigned SIZE>
00193    inline Matrix<DATA_TYPE, SIZE, SIZE>& preMult( Matrix<DATA_TYPE, SIZE, SIZE>& result,
00194                                                   const Matrix<DATA_TYPE, SIZE, SIZE>& operand )
00195    {
00196       return mult( result, operand, result );
00197    }
00198 
00204    template <typename DATA_TYPE, unsigned SIZE>
00205    inline Matrix<DATA_TYPE, SIZE, SIZE>& operator*=( Matrix<DATA_TYPE, SIZE, SIZE>& result,
00206                                                      const Matrix<DATA_TYPE, SIZE, SIZE>& operand )
00207    {
00208       return postMult( result, operand );
00209    }
00210 
00215    template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
00216    inline Matrix<DATA_TYPE, ROWS, COLS>& mult( Matrix<DATA_TYPE, ROWS, COLS>& result, const Matrix<DATA_TYPE, ROWS, COLS>& mat, float scalar )
00217    {
00218       for (unsigned i = 0; i < ROWS * COLS; ++i)
00219          result.mData[i] = mat.mData[i] * scalar;
00220       return result;
00221    }
00222 
00227    template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
00228    inline Matrix<DATA_TYPE, ROWS, COLS>& mult( Matrix<DATA_TYPE, ROWS, COLS>& result, DATA_TYPE scalar )
00229    {
00230       for (unsigned i = 0; i < ROWS * COLS; ++i)
00231          result.mData[i] *= scalar;
00232       return result;
00233    }
00234 
00239    template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
00240    inline Matrix<DATA_TYPE, ROWS, COLS>& operator*=( Matrix<DATA_TYPE, ROWS, COLS>& result, DATA_TYPE scalar )
00241    {
00242       return mult( result, scalar );
00243    }
00244 
00249    template <typename DATA_TYPE, unsigned SIZE>
00250    Matrix<DATA_TYPE, SIZE, SIZE>& transpose( Matrix<DATA_TYPE, SIZE, SIZE>& result )
00251    {
00252       // p. 27 game programming gems #1
00253       for (unsigned c = 0; c < SIZE; ++c)
00254          for (unsigned r = c + 1; r < SIZE; ++r)
00255             std::swap( result( r, c ), result( c, r ) );
00256 
00257       return result;
00258    }
00259 
00264    template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
00265    Matrix<DATA_TYPE, ROWS, COLS>& transpose( Matrix<DATA_TYPE, ROWS, COLS>& result, const Matrix<DATA_TYPE, COLS, ROWS>& source )
00266    {
00267       // in case result is == source... :(
00268       Matrix<DATA_TYPE, COLS, ROWS> temp = source;
00269 
00270       // p. 149 Numerical Analysis (second ed.)
00271       for (unsigned i = 0; i < ROWS; ++i)
00272       {
00273          for (unsigned j = 0; j < COLS; ++j)
00274          {
00275             result( i, j ) = temp( j, i );
00276          }
00277       }
00278 
00279       return result;
00280    }
00281 
00282 
00288    template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
00289    inline Matrix<DATA_TYPE, ROWS, COLS>& invertFull( Matrix<DATA_TYPE, ROWS, COLS>& result, const Matrix<DATA_TYPE, ROWS, COLS>& src )
00290    {
00291       /*---------------------------------------------------------------------------*
00292        | mat_inv: Compute the inverse of a n x n matrix, using the maximum pivot   |
00293        |          strategy.  n <= MAX1.                                            |
00294        *---------------------------------------------------------------------------*
00295 
00296          Parameters:
00297              a        a n x n square matrix
00298              b        inverse of input a.
00299              n        dimenstion of matrix a.
00300       */
00301 
00302       const DATA_TYPE* a = src.getData();
00303       DATA_TYPE* b = result.mData;
00304 
00305       int   n = 4;
00306       int    i, j, k;
00307       int    r[ 4], c[ 4], row[ 4], col[ 4];
00308       DATA_TYPE  m[ 4][ 4*2], pivot, max_m, tmp_m, fac;
00309 
00310       /* Initialization */
00311       for ( i = 0; i < n; i ++ )
00312       {
00313          r[ i] = c[ i] = 0;
00314          row[ i] = col[ i] = 0;
00315       }
00316 
00317       /* Set working matrix */
00318       for ( i = 0; i < n; i++ )
00319       {
00320          for ( j = 0; j < n; j++ )
00321          {
00322             m[ i][ j] = a[ i * n + j];
00323             m[ i][ j + n] = ( i == j ) ? (DATA_TYPE)1.0 : (DATA_TYPE)0.0 ;
00324          }
00325       }
00326 
00327       /* Begin of loop */
00328       for ( k = 0; k < n; k++ )
00329       {
00330          /* Choosing the pivot */
00331          for ( i = 0, max_m = 0; i < n; i++ )
00332          {
00333             if ( row[ i]  )
00334                continue;
00335             for ( j = 0; j < n; j++ )
00336             {
00337                if ( col[ j] )
00338                   continue;
00339                tmp_m = gmtl::Math::abs( m[ i][ j]);
00340                if ( tmp_m > max_m)
00341                {
00342                   max_m = tmp_m;
00343                   r[ k] = i;
00344                   c[ k] = j;
00345                }
00346             }
00347          }
00348          row[ r[k] ] = col[ c[k] ] = 1;
00349          pivot = m[ r[ k] ][ c[ k] ];
00350 
00351 
00352          if ( gmtl::Math::abs( pivot) <= 1e-20)
00353          {
00354             std::cerr << "*** pivot = " << pivot << " in mat_inv. ***\n";
00355             result.setError();
00356             return result;
00357          }
00358 
00359          /* Normalization */
00360          for ( j = 0; j < 2*n; j++ )
00361          {
00362             if ( j == c[ k] )
00363                m[ r[ k]][ j] = (DATA_TYPE)1.0;
00364             else
00365                m[ r[ k]][ j] /= pivot;
00366          }
00367 
00368          /* Reduction */
00369          for ( i = 0; i < n; i++ )
00370          {
00371             if ( i == r[ k] )
00372                continue;
00373 
00374             for ( j=0, fac = m[ i][ c[k]]; j < 2*n; j++ )
00375             {
00376                if ( j == c[ k] )
00377                   m[ i][ j] = (DATA_TYPE)0.0;
00378                else
00379                   m[ i][ j] -= fac * m[ r[k]][ j];
00380             }
00381          }
00382       }
00383 
00384       /* Assign inverse to a matrix */
00385       for ( i = 0; i < n; i++ )
00386          for ( j = 0; j < n; j++ )
00387             row[ i] = ( c[ j] == i ) ? r[ j] : row[ i];
00388 
00389       for ( i = 0; i < n; i++ )
00390          for ( j = 0; j < n; j++ )
00391             b[ i * n +  j] = m[ row[ i]][ j + n];
00392 
00393       // It worked
00394       return result;
00395    }
00396 
00407    template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
00408    inline Matrix<DATA_TYPE, ROWS, COLS>& invert( Matrix<DATA_TYPE, ROWS, COLS>& result, const Matrix<DATA_TYPE, ROWS, COLS>& src )
00409    {
00410       if (src.mState == Matrix<DATA_TYPE, ROWS, COLS>::IDENTITY )
00411          return result = src;
00412       else
00413          return invertFull( result, src );
00414    }
00415 
00426    template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
00427    inline Matrix<DATA_TYPE, ROWS, COLS>& invert( Matrix<DATA_TYPE, ROWS, COLS>& result )
00428    {
00429       return invert( result, result );
00430    }
00431 
00445    template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
00446    inline bool operator==( const Matrix<DATA_TYPE, ROWS, COLS>& lhs, const Matrix<DATA_TYPE, ROWS, COLS>& rhs )
00447    {
00448       for (unsigned int i = 0; i < ROWS*COLS; ++i)
00449       {
00450          if (lhs.mData[i] != rhs.mData[i])
00451          {
00452             return false;
00453          }
00454       }
00455 
00456       return true;
00457 
00458       /*  Would like this
00459       return( lhs[0] == rhs[0] &&
00460               lhs[1] == rhs[1] &&
00461               lhs[2] == rhs[2] );
00462       */
00463    }
00464 
00471    template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
00472    inline bool operator!=( const Matrix<DATA_TYPE, ROWS, COLS>& lhs, const Matrix<DATA_TYPE, ROWS, COLS>& rhs )
00473    {
00474       return bool( !(lhs == rhs) );
00475    }
00476 
00484    template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
00485    inline bool isEqual( const Matrix<DATA_TYPE, ROWS, COLS>& lhs, const Matrix<DATA_TYPE, ROWS, COLS>& rhs, const DATA_TYPE& eps = (DATA_TYPE)0 )
00486    {
00487       gmtlASSERT( eps >= (DATA_TYPE)0 );
00488 
00489       for (unsigned int i = 0; i < ROWS*COLS; ++i)
00490       {
00491          if (!Math::isEqual( lhs.mData[i], rhs.mData[i], eps ))
00492             return false;
00493       }
00494       return true;
00495    }
00498 } // end of namespace gmtl
00499 
00500 #endif