source: projectionDesigner/tag/ProjectionDesigner_1.1.5/projdesigner/include/gmtl/MatrixOps.h @ 2

Last change on this file since 2 was 2, checked in by Torben Dannhauer, 10 years ago
File size: 27.6 KB
Line 
1/************************************************************** ggt-head beg
2 *
3 * GGT: Generic Graphics Toolkit
4 *
5 * Original Authors:
6 *   Allen Bierbaum
7 *
8 * -----------------------------------------------------------------
9 * File:          MatrixOps.h,v
10 * Date modified: 2005/05/16 14:19:44
11 * Version:       1.39
12 * -----------------------------------------------------------------
13 *
14 *********************************************************** ggt-head end */
15/*************************************************************** ggt-cpr beg
16*
17* GGT: The Generic Graphics Toolkit
18* Copyright (C) 2001,2002 Allen Bierbaum
19*
20* This library is free software; you can redistribute it and/or
21* modify it under the terms of the GNU Lesser General Public
22* License as published by the Free Software Foundation; either
23* version 2.1 of the License, or (at your option) any later version.
24*
25* This library is distributed in the hope that it will be useful,
26* but WITHOUT ANY WARRANTY; without even the implied warranty of
27* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
28* Lesser General Public License for more details.
29*
30* You should have received a copy of the GNU Lesser General Public
31* License along with this library; if not, write to the Free Software
32* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
33*
34 ************************************************************ ggt-cpr end */
35#ifndef _GMTL_MATRIXOPS_H_
36#define _GMTL_MATRIXOPS_H_
37
38#include <iostream>         // for std::cerr
39#include <algorithm>        // needed for std::swap
40#include <gmtl/Matrix.h>
41#include <gmtl/Math.h>
42#include <gmtl/Vec.h>
43#include <gmtl/VecOps.h>
44#include <gmtl/Util/Assert.h>
45
46namespace gmtl
47{
48/** @ingroup Ops
49 * @name Matrix Operations
50 * @{
51 */
52
53   /** Make identity matrix out the matrix.
54    * @post Every element is 0 except the matrix's diagonal, whose elements are 1.
55    */
56   template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
57   inline Matrix<DATA_TYPE, ROWS, COLS>& identity( Matrix<DATA_TYPE, ROWS, COLS>& result )
58   {
59      if(result.mState != Matrix<DATA_TYPE, ROWS, COLS>::IDENTITY)   // if not already ident
60      {
61         // TODO: mp
62         for (unsigned int r = 0; r < ROWS; ++r)
63         for (unsigned int c = 0; c < COLS; ++c)
64            result( r, c ) = (DATA_TYPE)0.0;
65
66         // TODO: mp
67         for (unsigned int x = 0; x < Math::Min( COLS, ROWS ); ++x)
68            result( x, x ) = (DATA_TYPE)1.0;
69
70         result.mState = Matrix<DATA_TYPE, ROWS, COLS>::IDENTITY;
71//         result.mState = Matrix<DATA_TYPE, ROWS, COLS>::FULL;
72      }
73
74      return result;
75   }
76
77
78   /** zero out the matrix.
79    * @post every element is 0.
80    */
81   template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
82   inline Matrix<DATA_TYPE, ROWS, COLS>& zero( Matrix<DATA_TYPE, ROWS, COLS>& result )
83   {
84      if (result.mState == Matrix<DATA_TYPE, ROWS, COLS>::IDENTITY)
85      {
86         for (unsigned int x = 0; x < Math::Min( ROWS, COLS ); ++x)
87         {
88            result( x, x ) = (DATA_TYPE)0;
89         }
90      }
91      else
92      {
93         for (unsigned int x = 0; x < ROWS*COLS; ++x)
94         {
95            result.mData[x] = (DATA_TYPE)0;
96         }
97      }
98      result.mState = Matrix<DATA_TYPE, ROWS, COLS>::ORTHOGONAL;
99      return result;
100   }
101
102
103   /** matrix multiply.
104    *  @PRE: With regard to size (ROWS/COLS): if lhs is m x p, and rhs is p x n, then result is m x n (mult func undefined otherwise)
105    *  @POST: returns a m x n sized matrix
106    *  @post: result = lhs * rhs  (where rhs is applied first)
107    */
108   template <typename DATA_TYPE, unsigned ROWS, unsigned INTERNAL, unsigned COLS>
109   inline Matrix<DATA_TYPE, ROWS, COLS>& mult( Matrix<DATA_TYPE, ROWS, COLS>& result,
110                 const Matrix<DATA_TYPE, ROWS, INTERNAL>& lhs,
111                 const Matrix<DATA_TYPE, INTERNAL, COLS>& rhs )
112   {
113      Matrix<DATA_TYPE, ROWS, COLS> ret_mat; // prevent aliasing
114      zero( ret_mat );
115
116      // p. 150 Numerical Analysis (second ed.)
117      // if A is m x p, and B is p x n, then AB is m x n
118      // (AB)ij  =  [k = 1 to p] (a)ik (b)kj     (where:  1 <= i <= m, 1 <= j <= n)
119      for (unsigned int i = 0; i < ROWS; ++i)           // 1 <= i <= m
120      for (unsigned int j = 0; j < COLS; ++j)           // 1 <= j <= n
121      for (unsigned int k = 0; k < INTERNAL; ++k)       // [k = 1 to p]
122         ret_mat( i, j ) += lhs( i, k ) * rhs( k, j );
123
124      // track state
125      ret_mat.mState = combineMatrixStates( lhs.mState, rhs.mState );
126      return result = ret_mat;
127   }
128
129   /** matrix * matrix.
130    *  @PRE: With regard to size (ROWS/COLS): if lhs is m x p, and rhs is p x n, then result is m x n (mult func undefined otherwise)
131    *  @POST: returns a m x n sized matrix == lhs * rhs (where rhs is applied first)
132    *  returns a temporary, is slower.
133    */
134   template <typename DATA_TYPE, unsigned ROWS, unsigned INTERNAL, unsigned COLS>
135   inline Matrix<DATA_TYPE, ROWS, COLS> operator*( const Matrix<DATA_TYPE, ROWS, INTERNAL>& lhs,
136                                                   const Matrix<DATA_TYPE, INTERNAL, COLS>& rhs )
137   {
138      Matrix<DATA_TYPE, ROWS, COLS> temporary;
139      return mult( temporary, lhs, rhs );
140   }
141
142   /** matrix subtraction (algebraic operation for matrix).
143    *  @PRE: if lhs is m x n, and rhs is m x n, then result is m x n (mult func undefined otherwise)
144    *  @POST: returns a m x n matrix
145    *  @TODO: <B>enforce the sizes with templates...</b>
146    */
147   template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
148   inline Matrix<DATA_TYPE, ROWS, COLS>& sub( Matrix<DATA_TYPE, ROWS, COLS>& result,
149                 const Matrix<DATA_TYPE, ROWS, COLS>& lhs,
150                 const Matrix<DATA_TYPE, ROWS, COLS>& rhs )
151   {
152      // p. 150 Numerical Analysis (second ed.)
153      // if A is m x n, and B is m x n, then AB is m x n
154      // (A - B)ij  = (a)ij - (b)ij     (where:  1 <= i <= m, 1 <= j <= n)
155      for (unsigned int i = 0; i < ROWS; ++i)           // 1 <= i <= m
156      for (unsigned int j = 0; j < COLS; ++j)           // 1 <= j <= n
157         result( i, j ) = lhs( i, j ) - rhs( i, j );
158
159      // track state
160      result.mState = combineMatrixStates( lhs.mState, rhs.mState );
161      return result;
162   }
163
164   /** matrix addition (algebraic operation for matrix).
165    *  @PRE: if lhs is m x n, and rhs is m x n, then result is m x n (mult func undefined otherwise)
166    *  @POST: returns a m x n matrix
167    *  TODO: <B>enforce the sizes with templates...</b>
168    */
169   template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
170   inline Matrix<DATA_TYPE, ROWS, COLS>& add( Matrix<DATA_TYPE, ROWS, COLS>& result,
171                 const Matrix<DATA_TYPE, ROWS, COLS>& lhs,
172                 const Matrix<DATA_TYPE, ROWS, COLS>& rhs )
173   {
174      // p. 150 Numerical Analysis (second ed.)
175      // if A is m x n, and B is m x n, then AB is m x n
176      // (A - B)ij  = (a)ij + (b)ij     (where:  1 <= i <= m, 1 <= j <= n)
177      for (unsigned int i = 0; i < ROWS; ++i)           // 1 <= i <= m
178      for (unsigned int j = 0; j < COLS; ++j)           // 1 <= j <= n
179         result( i, j ) = lhs( i, j ) + rhs( i, j );
180
181      // track state
182      result.mState = combineMatrixStates( lhs.mState, rhs.mState );
183      return result;
184   }
185
186   /** matrix postmultiply.
187    * @PRE: args must both be n x n (this function is undefined otherwise)
188    * @POST: result' = result * operand
189    */
190   template <typename DATA_TYPE, unsigned SIZE>
191   inline Matrix<DATA_TYPE, SIZE, SIZE>& postMult( Matrix<DATA_TYPE, SIZE, SIZE>& result,
192                                                     const Matrix<DATA_TYPE, SIZE, SIZE>& operand )
193   {
194      return mult( result, result, operand );
195   }
196
197   /** matrix preMultiply.
198    * @PRE: args must both be n x n (this function is undefined otherwise)
199    * @POST: result' = operand * result
200    */
201   template <typename DATA_TYPE, unsigned SIZE>
202   inline Matrix<DATA_TYPE, SIZE, SIZE>& preMult( Matrix<DATA_TYPE, SIZE, SIZE>& result,
203                                                  const Matrix<DATA_TYPE, SIZE, SIZE>& operand )
204   {
205      return mult( result, operand, result );
206   }
207
208   /** matrix postmult (operator*=).
209    * does a postmult on the matrix.
210    * @PRE: args must both be n x n sized (this function is undefined otherwise)
211    * @POST: result' = result * operand  (where operand is applied first)
212    */
213   template <typename DATA_TYPE, unsigned SIZE>
214   inline Matrix<DATA_TYPE, SIZE, SIZE>& operator*=( Matrix<DATA_TYPE, SIZE, SIZE>& result,
215                                                     const Matrix<DATA_TYPE, SIZE, SIZE>& operand )
216   {
217      return postMult( result, operand );
218   }
219
220   /** matrix scalar mult.
221    *  mult each elt in a matrix by a scalar value.
222    *  @POST: result = mat * scalar
223    */
224   template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
225   inline Matrix<DATA_TYPE, ROWS, COLS>& mult( Matrix<DATA_TYPE, ROWS, COLS>& result, const Matrix<DATA_TYPE, ROWS, COLS>& mat, const DATA_TYPE& scalar )
226   {
227      for (unsigned i = 0; i < ROWS * COLS; ++i)
228         result.mData[i] = mat.mData[i] * scalar;
229      result.mState = mat.mState;
230      return result;
231   }
232
233   /** matrix scalar mult.
234    * mult each elt in a matrix by a scalar value.
235    *  @POST: result *= scalar
236    */
237   template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
238   inline Matrix<DATA_TYPE, ROWS, COLS>& mult( Matrix<DATA_TYPE, ROWS, COLS>& result, DATA_TYPE scalar )
239   {
240      for (unsigned i = 0; i < ROWS * COLS; ++i)
241         result.mData[i] *= scalar;
242      return result;
243   }
244
245   /** matrix scalar mult (operator*=).
246    * multiply matrix elements by a scalar
247    * @POST: result *= scalar
248    */
249   template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
250   inline Matrix<DATA_TYPE, ROWS, COLS>& operator*=( Matrix<DATA_TYPE, ROWS, COLS>& result, const DATA_TYPE& scalar )
251   {
252      return mult( result, scalar );
253   }
254
255   /** matrix transpose in place.
256    *  @PRE:  needs to be an N x N matrix
257    *  @POST: flip along diagonal
258    */
259   template <typename DATA_TYPE, unsigned SIZE>
260   Matrix<DATA_TYPE, SIZE, SIZE>& transpose( Matrix<DATA_TYPE, SIZE, SIZE>& result )
261   {
262      // p. 27 game programming gems #1
263      for (unsigned c = 0; c < SIZE; ++c)
264         for (unsigned r = c + 1; r < SIZE; ++r)
265            std::swap( result( r, c ), result( c, r ) );
266
267      return result;
268   }
269
270   /** matrix transpose from one type to another (i.e. 3x4 to 4x3)
271    *  @PRE:  source needs to be an M x N matrix, while dest needs to be N x M
272    *  @POST: flip along diagonal
273    */
274   template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
275   Matrix<DATA_TYPE, ROWS, COLS>& transpose( Matrix<DATA_TYPE, ROWS, COLS>& result, const Matrix<DATA_TYPE, COLS, ROWS>& source )
276   {
277      // in case result is == source... :(
278      Matrix<DATA_TYPE, COLS, ROWS> temp = source;
279
280      // p. 149 Numerical Analysis (second ed.)
281      for (unsigned i = 0; i < ROWS; ++i)
282      {
283         for (unsigned j = 0; j < COLS; ++j)
284         {
285            result( i, j ) = temp( j, i );
286         }
287      }
288      result.mState = temp.mState;
289      return result;
290   }
291
292   /** translational matrix inversion.
293    *  Matrix inversion that acts on a translational matrix (matrix with only translation)
294    *  Check for error with Matrix::isError().
295    * @pre: 4x3, 4x4 matrices only
296    * @post: result' = inv( result )
297    * @post: If inversion failed, then error bit is set within the Matrix.
298    */
299   template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
300   inline Matrix<DATA_TYPE, ROWS, COLS>& invertTrans( Matrix<DATA_TYPE, ROWS, COLS>& result,
301                                                       const Matrix<DATA_TYPE, ROWS, COLS>& src )
302   {
303      gmtlASSERT( ROWS == COLS || COLS == ROWS+1 && "invertTrans supports NxN or Nx(N-1) matrices only" );
304
305      if (&result != &src)
306         result = src; // could optimise this a little more (skip the trans copy), favor simplicity for now...
307      for (unsigned x = 0; x < (ROWS-1+(COLS-ROWS)); ++x)
308      {
309         result[x][3] = -result[x][3];
310      }
311      return result;
312   }
313
314   /** orthogonal matrix inversion.
315    *  Matrix inversion that acts on a affine matrix (matrix with only trans, rot, uniform scale)
316    *  Check for error with Matrix::isError().
317    * @pre: any size matrix
318    * @post: result' = inv( result )
319    * @post: If inversion failed, then error bit is set within the Matrix.
320    */
321   template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
322   inline Matrix<DATA_TYPE, ROWS, COLS>& invertOrthogonal( Matrix<DATA_TYPE, ROWS, COLS>& result,
323                                                       const Matrix<DATA_TYPE, ROWS, COLS>& src )
324   {
325      // in case result is == source... :(
326      Matrix<DATA_TYPE, ROWS, COLS> temp = src;
327
328      // if 3x4, 2x3, etc...  can't transpose the last column
329      const unsigned int size = Math::Min( ROWS, COLS );
330
331      // p. 149 Numerical Analysis (second ed.)
332      for (unsigned i = 0; i < size; ++i)
333      {
334         for (unsigned j = 0; j < size; ++j)
335         {
336            result( i, j ) = temp( j, i );
337         }
338      }
339      result.mState = temp.mState;
340      return result;
341   }
342
343   /** affine matrix inversion.
344    *  Matrix inversion that acts on a 4x3 affine matrix (matrix with only trans, rot, uniform scale)
345    *  Check for error with Matrix::isError().
346    * @pre: 3x3, 4x3, 4x4 matrices only
347    * @POST: result' = inv( result )
348    * @POST: If inversion failed, then error bit is set within the Matrix.
349    */
350   template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
351   inline Matrix<DATA_TYPE, ROWS, COLS>& invertAffine( Matrix<DATA_TYPE, ROWS, COLS>& result,
352                                                       const Matrix<DATA_TYPE, ROWS, COLS>& source )
353   {
354      static const float eps = 0.00000001f;
355
356      // in case &result is == &source... :(
357      Matrix<DATA_TYPE, ROWS, COLS> src = source;
358
359      // The rotational part of the matrix is simply the transpose of the
360      // original matrix.
361      for (int x = 0; x < 3; ++x)
362      for (int y = 0; y < 3; ++y)
363      {
364         result[x][y] = src[y][x];
365      }
366
367      // do non-uniform scale inversion
368      if (src.mState & Matrix<DATA_TYPE, ROWS, COLS>::NON_UNISCALE)
369      {
370         DATA_TYPE l0 = gmtl::lengthSquared( gmtl::Vec<DATA_TYPE, 3>( result[0][0], result[0][1], result[0][2] ) );
371         DATA_TYPE l1 = gmtl::lengthSquared( gmtl::Vec<DATA_TYPE, 3>( result[1][0], result[1][1], result[1][2] ) );
372         DATA_TYPE l2 = gmtl::lengthSquared( gmtl::Vec<DATA_TYPE, 3>( result[2][0], result[2][1], result[2][2] ) );
373         if (gmtl::Math::abs( l0 ) > eps) l0 = 1.0f / l0;
374         if (gmtl::Math::abs( l1 ) > eps) l1 = 1.0f / l1;
375         if (gmtl::Math::abs( l2 ) > eps) l2 = 1.0f / l2;
376         // apply the inverse scale to the 3x3
377         // for each axis: normalize it (1/length), and then mult by inverse scale (1/length)
378         result[0][0] *= l0;
379         result[0][1] *= l0;
380         result[0][2] *= l0;
381         result[1][0] *= l1;
382         result[1][1] *= l1;
383         result[1][2] *= l1;
384         result[2][0] *= l2;
385         result[2][1] *= l2;
386         result[2][2] *= l2;
387      }
388
389      // handle matrices with translation
390      if (COLS == 4)
391      {
392         // The right column vector of the matrix should always be [ 0 0 0 s ]
393         // this represents some shear values
394         result[3][0] = result[3][1] = result[3][2] = 0;
395
396         // The translation components of the original matrix.
397         const DATA_TYPE& tx = src[0][3];
398         const DATA_TYPE& ty = src[1][3];
399         const DATA_TYPE& tz = src[2][3];
400
401
402         // Rresult = -(Tm * Rm) to get the translation part of the inverse
403         if (ROWS == 4)
404         {
405            // invert scale.
406            const DATA_TYPE tw = (gmtl::Math::abs( src[3][3] ) > eps) ? 1.0f / src[3][3] : 0.0f;
407
408            // handle uniform scale in Nx4 matrices
409            result[0][3] = -( result[0][0] * tx + result[0][1] * ty + result[0][2] * tz ) * tw;
410            result[1][3] = -( result[1][0] * tx + result[1][1] * ty + result[1][2] * tz ) * tw;
411            result[2][3] = -( result[2][0] * tx + result[2][1] * ty + result[2][2] * tz ) * tw;
412            result[3][3] = tw;
413         }
414         else if (ROWS == 3)
415         {
416            result[0][3] = -( result[0][0] * tx + result[0][1] * ty + result[0][2] * tz );
417            result[1][3] = -( result[1][0] * tx + result[1][1] * ty + result[1][2] * tz );
418            result[2][3] = -( result[2][0] * tx + result[2][1] * ty + result[2][2] * tz );
419         }
420      }
421
422
423
424      result.mState = src.mState;
425
426      return result;
427   }
428
429   /** Full matrix inversion using Gauss-Jordan elimination.
430    *  Check for error with Matrix::isError().
431    * @POST: result' = inv( result )
432    * @POST: If inversion failed, then error bit is set within the Matrix.
433    */
434   template <typename DATA_TYPE, unsigned SIZE>
435   inline Matrix<DATA_TYPE, SIZE, SIZE>& invertFull_GJ( Matrix<DATA_TYPE, SIZE, SIZE>& result, const Matrix<DATA_TYPE, SIZE, SIZE>& src )
436   {
437      //gmtlASSERT( ROWS == COLS && "invertFull only works with nxn matrices" );
438
439      const DATA_TYPE pivot_eps(1e-20);         // Epsilon for the pivot value test (delta with test against zero)
440
441      // Computer inverse of matrix using a Gaussian-Jordan elimination.
442      // Uses max pivot at each point
443      // See: "Essential Mathmatics for Games" for description
444
445      // Do this invert in place
446      result = src;
447      unsigned swapped[SIZE];       // Track swaps. swapped[3] = 2 means that row 3 was swapped with row 2
448
449      unsigned pivot;
450
451      // --- Gaussian elimination step --- //
452      // For each column and row
453      for(pivot=0; pivot<SIZE;++pivot)
454      {
455         unsigned    pivot_row(pivot);
456         DATA_TYPE   pivot_value(gmtl::Math::abs(result(pivot_row, pivot)));    // Initialize to beginning of current row
457
458         // find pivot row - (max pivot element)
459         for(unsigned pr=pivot+1;pr<SIZE;++pr)
460         {
461            const DATA_TYPE cur_val(gmtl::Math::abs(result(pr,pivot)));   // get value at current point
462            if (cur_val > pivot_value)
463            {
464               pivot_row = pr;
465               pivot_value = cur_val;
466            }
467         }
468
469         if(gmtl::Math::isEqual(DATA_TYPE(0),pivot_value,pivot_eps))
470         {
471            std::cerr << "*** pivot = " << pivot_value << " in mat_inv. ***\n";
472            result.setError();
473            return result;
474         }
475
476         // Check for swap of pivot rows
477         swapped[pivot] = pivot_row;
478         if(pivot_row != pivot)
479         {
480            for(unsigned c=0;c<SIZE;++c)
481            {  std::swap(result(pivot,c), result(pivot_row,c)); }
482         }
483         // ASSERT: row to use in now in "row" (check that row starts with max pivot value found)
484         gmtlASSERT(gmtl::Math::isEqual(pivot_value,gmtl::Math::abs(result(pivot,pivot)),DATA_TYPE(0.00001)));
485
486         // Compute pivot factor
487         const DATA_TYPE mult_factor(1.0f/pivot_value);
488
489         // Set pivot row values
490         for(unsigned c=0;c<SIZE;++c)
491         {  result(pivot,c) *= mult_factor; }
492         result(pivot,pivot) = mult_factor;    // Copy the 1/pivot since we are inverting in place
493
494         // Clear pivot column in other rows (since we are in place)
495         // - Subtract current row times result(r,col) so that column element becomes 0
496         for(unsigned row=0;row<SIZE;++row)
497         {
498            if(row==pivot)     // Don't subtract from our row
499            { continue; }
500
501            const DATA_TYPE sub_mult_factor(result(row,pivot));
502
503            // Clear the pivot column's element (for invers in place)
504            // ends up being set to -sub_mult_factor*pivotInverse
505            result(row,pivot) = 0;
506
507            // subtract the pivot row from this row
508            for(unsigned col=0;col<SIZE;++col)
509            {   result(row,col) -= (sub_mult_factor*result(pivot,col)); }
510         }
511      } // end: gaussian substitution
512
513
514      // Now undo the swaps in column direction in reverse order
515      unsigned p(SIZE);
516      do
517      {
518         --p;
519         gmtlASSERT(p<SIZE);
520
521         // If row was swapped
522         if(swapped[p] != p)
523         {
524            // Swap the column with same index
525            for(unsigned r=0; r<SIZE; ++r)
526            { std::swap(result(r, p), result(r, swapped[p])); }
527         }
528      }
529      while(p>0);
530
531      return result;
532   }
533
534
535   /** full matrix inversion.
536    *  Check for error with Matrix::isError().
537    * @POST: result' = inv( result )
538    * @POST: If inversion failed, then error bit is set within the Matrix.
539    */
540   template <typename DATA_TYPE, unsigned SIZE>
541   inline Matrix<DATA_TYPE, SIZE, SIZE>& invertFull_orig( Matrix<DATA_TYPE, SIZE, SIZE>& result, const Matrix<DATA_TYPE, SIZE, SIZE>& src )
542   {
543      /*---------------------------------------------------------------------------*
544       | mat_inv: Compute the inverse of a n x n matrix, using the maximum pivot   |
545       |          strategy.  n <= MAX1.                                            |
546       *---------------------------------------------------------------------------*
547
548         Parameters:
549             a        a n x n square matrix
550             b        inverse of input a.
551             n        dimenstion of matrix a.
552      */
553
554      const DATA_TYPE* a = src.getData();
555      DATA_TYPE* b = result.mData;
556
557      int   n(SIZE);
558      int    i, j, k;
559      int    r[SIZE], c[SIZE], row[SIZE], col[SIZE];
560      DATA_TYPE  m[SIZE][SIZE*2], pivot, max_m, tmp_m, fac;
561
562      /* Initialization */
563      for ( i = 0; i < n; i ++ )
564      {
565         r[ i] = c[ i] = 0;
566         row[ i] = col[ i] = 0;
567      }
568
569      /* Set working matrix */
570      for ( i = 0; i < n; i++ )
571      {
572         for ( j = 0; j < n; j++ )
573         {
574            m[ i][ j] = a[ i * n + j];
575            m[ i][ j + n] = ( i == j ) ? (DATA_TYPE)1.0 : (DATA_TYPE)0.0 ;
576         }
577      }
578
579      /* Begin of loop */
580      for ( k = 0; k < n; k++ )
581      {
582         /* Choosing the pivot */
583         for ( i = 0, max_m = 0; i < n; i++ )
584         {
585            if ( row[ i]  )
586               continue;
587            for ( j = 0; j < n; j++ )
588            {
589               if ( col[ j] )
590                  continue;
591               tmp_m = gmtl::Math::abs( m[ i][ j]);
592               if ( tmp_m > max_m)
593               {
594                  max_m = tmp_m;
595                  r[ k] = i;
596                  c[ k] = j;
597               }
598            }
599         }
600         row[ r[k] ] = col[ c[k] ] = 1;
601         pivot = m[ r[ k] ][ c[ k] ];
602
603
604         if ( gmtl::Math::abs( pivot) <= 1e-20)
605         {
606            std::cerr << "*** pivot = " << pivot << " in mat_inv. ***\n";
607            result.setError();
608            return result;
609         }
610
611         /* Normalization */
612         for ( j = 0; j < 2*n; j++ )
613         {
614            if ( j == c[ k] )
615               m[ r[ k]][ j] = (DATA_TYPE)1.0;
616            else
617               m[ r[ k]][ j] /= pivot;
618         }
619
620         /* Reduction */
621         for ( i = 0; i < n; i++ )
622         {
623            if ( i == r[ k] )
624               continue;
625
626            for ( j=0, fac = m[ i][ c[k]]; j < 2*n; j++ )
627            {
628               if ( j == c[ k] )
629                  m[ i][ j] = (DATA_TYPE)0.0;
630               else
631                  m[ i][ j] -= fac * m[ r[k]][ j];
632            }
633         }
634      }
635
636      /* Assign inverse to a matrix */
637      for ( i = 0; i < n; i++ )
638         for ( j = 0; j < n; j++ )
639            row[ i] = ( c[ j] == i ) ? r[ j] : row[ i];
640
641      for ( i = 0; i < n; i++ )
642         for ( j = 0; j < n; j++ )
643            b[ i * n +  j] = m[ row[ i]][ j + n];
644
645      // It worked
646      result.mState = src.mState;
647      return result;
648   }
649
650
651   /** Invert method.
652    * Calls invertFull_orig to do the work.
653    */
654   template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
655   inline Matrix<DATA_TYPE, ROWS, COLS>& invertFull( Matrix<DATA_TYPE, ROWS, COLS>& result, const Matrix<DATA_TYPE, ROWS, COLS>& src )
656   {
657      return invertFull_orig(result,src);
658   }
659
660   /** smart matrix inversion.
661    *  Does matrix inversion by intelligently selecting what type of inversion to use depending
662    *  on the types of operations your Matrix has been through.
663    *
664    *  5 types of inversion: FULL, AFFINE, ORTHONORMAL, ORTHOGONAL, IDENTITY.
665    *
666    *  Check for error with Matrix::isError().
667    * @POST: result' = inv( result )
668    * @POST: If inversion failed, then error bit is set within the Matrix.
669    */
670   template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
671   inline Matrix<DATA_TYPE, ROWS, COLS>& invert( Matrix<DATA_TYPE, ROWS, COLS>& result, const Matrix<DATA_TYPE, ROWS, COLS>& src )
672   {
673      if (src.mState == Matrix<DATA_TYPE, ROWS, COLS>::IDENTITY )
674         return result = src;
675      else if (src.mState == Matrix<DATA_TYPE, ROWS, COLS>::TRANS)
676         return invertTrans( result, src );
677      else if (src.mState == Matrix<DATA_TYPE, ROWS, COLS>::ORTHOGONAL)
678         return invertOrthogonal( result, src );
679      else if (src.mState == Matrix<DATA_TYPE, ROWS, COLS>::AFFINE ||
680               src.mState == (Matrix<DATA_TYPE, ROWS, COLS>::AFFINE | Matrix<DATA_TYPE, ROWS, COLS>::NON_UNISCALE))
681         return invertAffine( result, src );
682      else
683         return invertFull_orig( result, src );
684   }
685
686   /** smart matrix inversion (in place)
687    *  Does matrix inversion by intelligently selecting what type of inversion to use depending
688    *  on the types of operations your Matrix has been through.
689    *
690    *  5 types of inversion: FULL, AFFINE, ORTHONORMAL, ORTHOGONAL, IDENTITY.
691    *
692    *  Check for error with Matrix::isError().
693    * @POST: result' = inv( result )
694    * @POST: If inversion failed, then error bit is set within the Matrix.
695    */
696   template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
697   inline Matrix<DATA_TYPE, ROWS, COLS>& invert( Matrix<DATA_TYPE, ROWS, COLS>& result )
698   {
699      return invert( result, result );
700   }
701
702/** @} */
703
704/** @ingroup Compare
705 * @name Matrix Comparitors
706 * @{
707 */
708
709   /** Tests 2 matrices for equality
710    *  @param lhs    The first matrix
711    *  @param rhs    The second matrix
712    *  @pre Both matrices must be of the same size.
713    *  @return true if the matrices have the same element values; false otherwise
714    */
715   template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
716   inline bool operator==( const Matrix<DATA_TYPE, ROWS, COLS>& lhs, const Matrix<DATA_TYPE, ROWS, COLS>& rhs )
717   {
718      for (unsigned int i = 0; i < ROWS*COLS; ++i)
719      {
720         if (lhs.mData[i] != rhs.mData[i])
721         {
722            return false;
723         }
724      }
725
726      return true;
727
728      /*  Would like this
729      return( lhs[0] == rhs[0] &&
730              lhs[1] == rhs[1] &&
731              lhs[2] == rhs[2] );
732      */
733   }
734
735   /** Tests 2 matrices for inequality
736    *  @param lhs    The first matrix
737    *  @param rhs    The second matrix
738    *  @pre Both matrices must be of the same size.
739    *  @return false if the matrices differ on any element value; true otherwise
740    */
741   template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
742   inline bool operator!=( const Matrix<DATA_TYPE, ROWS, COLS>& lhs, const Matrix<DATA_TYPE, ROWS, COLS>& rhs )
743   {
744      return bool( !(lhs == rhs) );
745   }
746
747   /** Tests 2 matrices for equality within a tolerance
748    *  @param lhs    The first matrix
749    *  @param rhs    The second matrix
750    *  @param eps    The tolerance value
751    *  @pre Both matrices must be of the same size.
752    *  @return true if the matrices' elements are within the tolerance value of each other; false otherwise
753    */
754   template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
755   inline bool isEqual( const Matrix<DATA_TYPE, ROWS, COLS>& lhs, const Matrix<DATA_TYPE, ROWS, COLS>& rhs, const DATA_TYPE eps = 0 )
756   {
757      gmtlASSERT( eps >= (DATA_TYPE)0 );
758
759      for (unsigned int i = 0; i < ROWS*COLS; ++i)
760      {
761         if (!Math::isEqual( lhs.mData[i], rhs.mData[i], eps ))
762            return false;
763      }
764      return true;
765   }
766/** @} */
767
768} // end of namespace gmtl
769
770#endif
Note: See TracBrowser for help on using the repository browser.