LCOV - code coverage report
Current view: directory - alg - thinplatespline.cpp (source / functions) Found Hit Coverage
Test: gdal_filtered.info Lines: 223 113 50.7 %
Date: 2012-12-26 Functions: 12 8 66.7 %

       1                 : /******************************************************************************
       2                 :  * $Id: thinplatespline.cpp 24925 2012-09-16 10:07:00Z rouault $
       3                 :  *
       4                 :  * Project:  GDAL Warp API
       5                 :  * Purpose:  Implemenentation of 2D Thin Plate Spline transformer. 
       6                 :  * Author:   VIZRT Development Team.
       7                 :  *
       8                 :  * This code was provided by Gilad Ronnen (gro at visrt dot com) with
       9                 :  * permission to reuse under the following license.
      10                 :  * 
      11                 :  ******************************************************************************
      12                 :  * Copyright (c) 2004, VIZRT Inc.
      13                 :  *
      14                 :  * Permission is hereby granted, free of charge, to any person obtaining a
      15                 :  * copy of this software and associated documentation files (the "Software"),
      16                 :  * to deal in the Software without restriction, including without limitation
      17                 :  * the rights to use, copy, modify, merge, publish, distribute, sublicense,
      18                 :  * and/or sell copies of the Software, and to permit persons to whom the
      19                 :  * Software is furnished to do so, subject to the following conditions:
      20                 :  *
      21                 :  * The above copyright notice and this permission notice shall be included
      22                 :  * in all copies or substantial portions of the Software.
      23                 :  *
      24                 :  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
      25                 :  * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
      26                 :  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
      27                 :  * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
      28                 :  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
      29                 :  * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
      30                 :  * DEALINGS IN THE SOFTWARE.
      31                 :  ****************************************************************************/
      32                 : 
      33                 : #ifdef HAVE_ARMADILLO
      34                 : /* Include before #define A(r,c) because armadillo uses A in its include files */
      35                 : #include "armadillo"
      36                 : #endif
      37                 : 
      38                 : #include "thinplatespline.h"
      39                 : 
      40                 : #ifdef HAVE_FLOAT_H
      41                 : #  include <float.h>
      42                 : #elif defined(HAVE_VALUES_H)
      43                 : #  include <values.h>
      44                 : #endif
      45                 : 
      46                 : #ifndef FLT_MAX
      47                 : #  define FLT_MAX 1e+37
      48                 : #  define FLT_MIN 1e-37
      49                 : #endif
      50                 : 
      51                 : VizGeorefSpline2D* viz_xy2llz;
      52                 : VizGeorefSpline2D* viz_llz2xy;
      53                 : 
      54                 : /////////////////////////////////////////////////////////////////////////////////////
      55                 : //// vizGeorefSpline2D
      56                 : /////////////////////////////////////////////////////////////////////////////////////
      57                 : 
      58                 : #define A(r,c) _AA[ _nof_eqs * (r) + (c) ]
      59                 : #define Ainv(r,c) _Ainv[ _nof_eqs * (r) + (c) ]
      60                 : 
      61                 : 
      62                 : #define VIZ_GEOREF_SPLINE_DEBUG 0
      63                 : 
      64                 : static int matrixInvert( int N, double input[], double output[] );
      65                 : 
      66              24 : void VizGeorefSpline2D::grow_points()
      67                 : 
      68                 : {
      69              24 :     int new_max = _max_nof_points*2 + 2 + 3;
      70                 :     int i;
      71                 :     
      72              24 :     if( _max_nof_points == 0 )
      73                 :     {
      74              12 :         x = (double *) VSIMalloc( sizeof(double) * new_max );
      75              12 :         y = (double *) VSIMalloc( sizeof(double) * new_max );
      76              12 :         u = (double *) VSIMalloc( sizeof(double) * new_max );
      77              12 :         unused = (int *) VSIMalloc( sizeof(int) * new_max );
      78              12 :         index = (int *) VSIMalloc( sizeof(int) * new_max );
      79              36 :         for( i = 0; i < VIZGEOREF_MAX_VARS; i++ )
      80                 :         {
      81              24 :             rhs[i] = (double *) VSICalloc( sizeof(double), new_max );
      82              24 :             coef[i] = (double *) VSICalloc( sizeof(double), new_max );
      83                 :         }
      84                 :     }
      85                 :     else
      86                 :     {
      87              12 :         x = (double *) VSIRealloc( x, sizeof(double) * new_max );
      88              12 :         y = (double *) VSIRealloc( y, sizeof(double) * new_max );
      89              12 :         u = (double *) VSIRealloc( u, sizeof(double) * new_max );
      90              12 :         unused = (int *) VSIRealloc( unused, sizeof(int) * new_max );
      91              12 :         index = (int *) VSIRealloc( index, sizeof(int) * new_max );
      92              36 :         for( i = 0; i < VIZGEOREF_MAX_VARS; i++ )
      93                 :         {
      94              24 :             rhs[i] = (double *) 
      95              24 :                 VSIRealloc( rhs[i], sizeof(double) * new_max );
      96              24 :             coef[i] = (double *) 
      97              24 :                 VSIRealloc( coef[i], sizeof(double) * new_max );
      98                 :         }
      99                 :     }
     100                 : 
     101              24 :     _max_nof_points = new_max - 3;
     102              24 : }
     103                 : 
     104              46 : int VizGeorefSpline2D::add_point( const double Px, const double Py, const double *Pvars )
     105                 : {
     106              46 :     type = VIZ_GEOREF_SPLINE_POINT_WAS_ADDED;
     107                 :     int i;
     108                 : 
     109              46 :     if( _nof_points == _max_nof_points )
     110              12 :         grow_points();
     111                 : 
     112              46 :     i = _nof_points;
     113                 :     //A new point is added
     114              46 :     x[i] = Px;
     115              46 :     y[i] = Py;
     116             138 :     for ( int j = 0; j < _nof_vars; j++ )
     117              92 :         rhs[j][i+3] = Pvars[j];
     118              46 :     _nof_points++;
     119              46 :     return 1;
     120                 : }
     121                 : 
     122               0 : bool VizGeorefSpline2D::change_point(int index, double Px, double Py, double* Pvars)
     123                 : {
     124               0 :     if ( index < _nof_points )
     125                 :     {
     126               0 :         int i = index;
     127               0 :         x[i] = Px;
     128               0 :         y[i] = Py;
     129               0 :         for ( int j = 0; j < _nof_vars; j++ )
     130               0 :             rhs[j][i+3] = Pvars[j];
     131                 :     }
     132                 : 
     133               0 :     return( true );
     134                 : }
     135                 : 
     136               0 : bool VizGeorefSpline2D::get_xy(int index, double& outX, double& outY)
     137                 : {
     138                 :     bool ok;
     139                 : 
     140               0 :     if ( index < _nof_points )
     141                 :     {
     142               0 :         ok = true;
     143               0 :         outX = x[index];
     144               0 :         outY = y[index];
     145                 :     }
     146                 :     else
     147                 :     {
     148               0 :         ok = false;
     149               0 :         outX = outY = 0.0f;
     150                 :     }
     151                 : 
     152               0 :     return(ok);
     153                 : }
     154                 : 
     155               0 : int VizGeorefSpline2D::delete_point(const double Px, const double Py )
     156                 : {
     157               0 :     for ( int i = 0; i < _nof_points; i++ )
     158                 :     {
     159               0 :         if ( ( fabs(Px - x[i]) <= _tx ) && ( fabs(Py - y[i]) <= _ty ) )
     160                 :         {
     161               0 :             for ( int j = i; j < _nof_points - 1; j++ )
     162                 :             {
     163               0 :                 x[j] = x[j+1];
     164               0 :                 y[j] = y[j+1];
     165               0 :                 for ( int k = 0; k < _nof_vars; k++ )
     166               0 :                     rhs[k][j+3] = rhs[k][j+3+1];
     167                 :             }
     168               0 :             _nof_points--;
     169               0 :             type = VIZ_GEOREF_SPLINE_POINT_WAS_DELETED;
     170               0 :             return(1);
     171                 :         }
     172                 :     }
     173               0 :     return(0);
     174                 : }
     175                 : 
     176              12 : int VizGeorefSpline2D::solve(void)
     177                 : {
     178                 :     int r, c, v;
     179                 :     int p;
     180                 :   
     181                 :     //  No points at all
     182              12 :     if ( _nof_points < 1 )
     183                 :     {
     184               0 :         type = VIZ_GEOREF_SPLINE_ZERO_POINTS;
     185               0 :         return(0);
     186                 :     }
     187                 :   
     188                 :     // Only one point
     189              12 :     if ( _nof_points == 1 )
     190                 :     {
     191               0 :         type = VIZ_GEOREF_SPLINE_ONE_POINT;
     192               0 :         return(1);
     193                 :     }
     194                 :     // Just 2 points - it is necessarily 1D case
     195              12 :     if ( _nof_points == 2 )
     196                 :     {
     197               0 :         _dx = x[1] - x[0];
     198               0 :         _dy = y[1] - y[0];   
     199               0 :         double fact = 1.0 / ( _dx * _dx + _dy * _dy );
     200               0 :         _dx *= fact;
     201               0 :         _dy *= fact;
     202                 :     
     203               0 :         type = VIZ_GEOREF_SPLINE_TWO_POINTS;
     204               0 :         return(2);
     205                 :     }
     206                 :   
     207                 :     // More than 2 points - first we have to check if it is 1D or 2D case
     208                 :     
     209              12 :     double xmax = x[0], xmin = x[0], ymax = y[0], ymin = y[0];
     210                 :     double delx, dely;
     211                 :     double xx, yy;
     212              12 :     double sumx = 0.0f, sumy= 0.0f, sumx2 = 0.0f, sumy2 = 0.0f, sumxy = 0.0f;
     213                 :     double SSxx, SSyy, SSxy;
     214                 :   
     215              58 :     for ( p = 0; p < _nof_points; p++ )
     216                 :     {
     217              46 :         xx = x[p];
     218              46 :         yy = y[p];
     219                 :     
     220              46 :         xmax = MAX( xmax, xx );
     221              46 :         xmin = MIN( xmin, xx );
     222              46 :         ymax = MAX( ymax, yy );
     223              46 :         ymin = MIN( ymin, yy );
     224                 :     
     225              46 :         sumx  += xx;
     226              46 :         sumx2 += xx * xx;
     227              46 :         sumy  += yy;
     228              46 :         sumy2 += yy * yy;
     229              46 :         sumxy += xx * yy;
     230                 :     }
     231              12 :     delx = xmax - xmin;
     232              12 :     dely = ymax - ymin;
     233                 :   
     234              12 :     SSxx = sumx2 - sumx * sumx / _nof_points;
     235              12 :     SSyy = sumy2 - sumy * sumy / _nof_points;
     236              12 :     SSxy = sumxy - sumx * sumy / _nof_points;
     237                 :   
     238              12 :     if ( delx < 0.001 * dely || dely < 0.001 * delx || 
     239                 :          fabs ( SSxy * SSxy / ( SSxx * SSyy ) ) > 0.99 )
     240                 :     {
     241                 :         int p1;
     242                 :     
     243               0 :         type = VIZ_GEOREF_SPLINE_ONE_DIMENSIONAL;
     244                 :     
     245               0 :         _dx = _nof_points * sumx2 - sumx * sumx;
     246               0 :         _dy = _nof_points * sumy2 - sumy * sumy;
     247               0 :         double fact = 1.0 / sqrt( _dx * _dx + _dy * _dy );
     248               0 :         _dx *= fact;
     249               0 :         _dy *= fact;
     250                 :     
     251               0 :         for ( p = 0; p < _nof_points; p++ )
     252                 :         {
     253               0 :             double dxp = x[p] - x[0];
     254               0 :             double dyp = y[p] - y[0];
     255               0 :             u[p] = _dx * dxp + _dy * dyp;
     256               0 :             unused[p] = 1;
     257                 :         }
     258                 :     
     259               0 :         for ( p = 0; p < _nof_points; p++ )
     260                 :         {
     261               0 :             int min_index = -1;
     262               0 :             double min_u = 0;
     263               0 :             for ( p1 = 0; p1 < _nof_points; p1++ )
     264                 :             {
     265               0 :                 if ( unused[p1] )
     266                 :                 {
     267               0 :                     if ( min_index < 0 || u[p1] < min_u )
     268                 :                     {
     269               0 :                         min_index = p1;
     270               0 :                         min_u = u[p1];
     271                 :                     }
     272                 :                 }
     273                 :             }
     274               0 :             index[p] = min_index;
     275               0 :             unused[min_index] = 0;
     276                 :         }
     277                 :     
     278               0 :         return(3);
     279                 :     }
     280                 :   
     281              12 :     type = VIZ_GEOREF_SPLINE_FULL;
     282                 :     // Make the necessary memory allocations
     283              12 :     if ( _AA )
     284               0 :         CPLFree(_AA);
     285              12 :     if ( _Ainv )
     286               0 :         CPLFree(_Ainv);
     287                 :   
     288              12 :     _nof_eqs = _nof_points + 3;
     289                 :     
     290              12 :     if( _nof_eqs > INT_MAX / _nof_eqs )
     291                 :     {
     292               0 :         fprintf(stderr, "Too many coefficients. Computation aborted.\n");
     293               0 :         return 0;
     294                 :     }
     295                 :   
     296              12 :     _AA = ( double * )VSICalloc( _nof_eqs * _nof_eqs, sizeof( double ) );
     297              12 :     _Ainv = ( double * )VSICalloc( _nof_eqs * _nof_eqs, sizeof( double ) );
     298                 :     
     299              12 :     if( _AA == NULL || _Ainv == NULL )
     300                 :     {
     301               0 :         fprintf(stderr, "Out-of-memory while allocating temporary arrays. Computation aborted.\n");
     302               0 :         return 0;
     303                 :     }
     304                 :   
     305                 :     // Calc the values of the matrix A
     306              48 :     for ( r = 0; r < 3; r++ )
     307             144 :         for ( c = 0; c < 3; c++ )
     308             108 :             A(r,c) = 0.0;
     309                 :     
     310              58 :     for ( c = 0; c < _nof_points; c++ )
     311                 :     {
     312              46 :         A(0,c+3) = 1.0;
     313              46 :         A(1,c+3) = x[c];
     314              46 :         A(2,c+3) = y[c];
     315                 :       
     316              46 :         A(c+3,0) = 1.0;
     317              46 :         A(c+3,1) = x[c];
     318              46 :         A(c+3,2) = y[c];
     319                 :     }
     320                 :     
     321              58 :     for ( r = 0; r < _nof_points; r++ )
     322             158 :         for ( c = r; c < _nof_points; c++ )
     323                 :         {
     324             112 :             A(r+3,c+3) = base_func( x[r], y[r], x[c], y[c] );
     325             112 :             if ( r != c )
     326              66 :                 A(c+3,r+3 ) = A(r+3,c+3);
     327                 :         }
     328                 :       
     329                 : #if VIZ_GEOREF_SPLINE_DEBUG
     330                 :       
     331                 :     for ( r = 0; r < _nof_eqs; r++ )
     332                 :     {
     333                 :         for ( c = 0; c < _nof_eqs; c++ )
     334                 :             fprintf(stderr, "%f", A(r,c));
     335                 :         fprintf(stderr, "\n");
     336                 :     }
     337                 :       
     338                 : #endif
     339                 :       
     340                 :     // Invert the matrix
     341              12 :     int status = matrixInvert( _nof_eqs, _AA, _Ainv );
     342                 :       
     343              12 :     if ( !status )
     344                 :     {
     345               0 :         fprintf(stderr, " There is a problem to invert the interpolation matrix\n");
     346               0 :         return 0;
     347                 :     }
     348                 :       
     349                 :     // calc the coefs
     350              36 :     for ( v = 0; v < _nof_vars; v++ )
     351             188 :         for ( r = 0; r < _nof_eqs; r++ )
     352                 :         {
     353             164 :             coef[v][r] = 0.0;
     354            1288 :             for ( c = 0; c < _nof_eqs; c++ )
     355            1124 :                 coef[v][r] += Ainv(r,c) * rhs[v][c];
     356                 :         }
     357                 :         
     358              12 :     return(4);
     359                 : }
     360                 : 
     361            2564 : int VizGeorefSpline2D::get_point( const double Px, const double Py, double *vars )
     362                 : {
     363                 :   int v, r;
     364                 :   double tmp, Pu;
     365                 :   double fact;
     366            2564 :   int leftP=0, rightP=0, found = 0;
     367                 :   
     368            2564 :   switch ( type )
     369                 :   {
     370                 :   case VIZ_GEOREF_SPLINE_ZERO_POINTS :
     371               0 :     for ( v = 0; v < _nof_vars; v++ )
     372               0 :       vars[v] = 0.0;
     373               0 :     break;
     374                 :   case VIZ_GEOREF_SPLINE_ONE_POINT :
     375               0 :     for ( v = 0; v < _nof_vars; v++ )
     376               0 :       vars[v] = rhs[v][3];
     377               0 :     break;
     378                 :   case VIZ_GEOREF_SPLINE_TWO_POINTS :
     379               0 :     fact = _dx * ( Px - x[0] ) + _dy * ( Py - y[0] );
     380               0 :     for ( v = 0; v < _nof_vars; v++ )
     381               0 :       vars[v] = ( 1 - fact ) * rhs[v][3] + fact * rhs[v][4];
     382               0 :     break;
     383                 :   case VIZ_GEOREF_SPLINE_ONE_DIMENSIONAL :
     384               0 :     Pu = _dx * ( Px - x[0] ) + _dy * ( Py - y[0] );
     385               0 :     if ( Pu <= u[index[0]] )
     386                 :     {
     387               0 :       leftP = index[0];
     388               0 :       rightP = index[1];
     389                 :     }
     390               0 :     else if ( Pu >= u[index[_nof_points-1]] )
     391                 :     {
     392               0 :       leftP = index[_nof_points-2];
     393               0 :       rightP = index[_nof_points-1];
     394                 :     }
     395                 :     else
     396                 :     {
     397               0 :       for ( r = 1; !found && r < _nof_points; r++ )
     398                 :       {
     399               0 :         leftP = index[r-1];
     400               0 :         rightP = index[r];          
     401               0 :         if ( Pu >= u[leftP] && Pu <= u[rightP] )
     402               0 :           found = 1;
     403                 :       }
     404                 :     }
     405                 :     
     406               0 :     fact = ( Pu - u[leftP] ) / ( u[rightP] - u[leftP] );
     407               0 :     for ( v = 0; v < _nof_vars; v++ )
     408               0 :       vars[v] = ( 1.0 - fact ) * rhs[v][leftP+3] +
     409               0 :       fact * rhs[v][rightP+3];
     410               0 :     break;
     411                 :   case VIZ_GEOREF_SPLINE_FULL :
     412            7692 :     for ( v = 0; v < _nof_vars; v++ )
     413            5128 :       vars[v] = coef[v][0] + coef[v][1] * Px + coef[v][2] * Py;
     414                 :     
     415           12818 :     for ( r = 0; r < _nof_points; r++ )
     416                 :     {
     417           10254 :       tmp = base_func( Px, Py, x[r], y[r] );
     418           30762 :       for ( v= 0; v < _nof_vars; v++ )
     419           20508 :         vars[v] += coef[v][r+3] * tmp;
     420                 :     }
     421            2564 :     break;
     422                 :   case VIZ_GEOREF_SPLINE_POINT_WAS_ADDED :
     423               0 :     fprintf(stderr, " A point was added after the last solve\n");
     424               0 :     fprintf(stderr, " NO interpolation - return values are zero\n");
     425               0 :     for ( v = 0; v < _nof_vars; v++ )
     426               0 :       vars[v] = 0.0;
     427               0 :     return(0);
     428                 :     break;
     429                 :   case VIZ_GEOREF_SPLINE_POINT_WAS_DELETED :
     430               0 :     fprintf(stderr, " A point was deleted after the last solve\n");
     431               0 :     fprintf(stderr, " NO interpolation - return values are zero\n");
     432               0 :     for ( v = 0; v < _nof_vars; v++ )
     433               0 :       vars[v] = 0.0;
     434               0 :     return(0);
     435                 :     break;
     436                 :   default :
     437               0 :     return(0);
     438                 :     break;
     439                 :   }
     440            2564 :   return(1);
     441                 : }
     442                 : 
     443           10366 : double VizGeorefSpline2D::base_func( const double x1, const double y1,
     444                 :               const double x2, const double y2 )
     445                 : {
     446           10366 :   if ( ( x1 == x2 ) && (y1 == y2 ) )
     447             106 :     return 0.0;
     448                 :   
     449           10260 :   double dist  = ( x2 - x1 ) * ( x2 - x1 ) + ( y2 - y1 ) * ( y2 - y1 );
     450                 :   
     451           10260 :   return dist * log( dist );  
     452                 : }
     453                 : 
     454                 : #ifdef HAVE_ARMADILLO
     455                 : 
     456              12 : static int matrixInvert( int N, double input[], double output[] )
     457                 : {
     458                 :     try
     459                 :     {
     460              12 :         arma::mat matInput(input,N,N,false);
     461              12 :         const arma::mat& matInv = arma::inv(matInput);
     462                 :         int row, col;
     463              94 :         for(row = 0; row < N; row++)
     464             644 :             for(col = 0; col < N; col++)
     465            1124 :                 output[row * N + col] = matInv.at(row, col);
     466              12 :         return true;
     467                 :         //arma::mat matInv(output,N,N,false);
     468                 :         //return arma::inv(matInv, matInput);
     469                 :     }
     470               0 :     catch(...)
     471                 :     {
     472               0 :         fprintf(stderr, "matrixInvert(): error occured.\n");
     473               0 :         return false;
     474                 :     }
     475            2139 : }
     476                 : 
     477                 : #else
     478                 : 
     479                 : static int matrixInvert( int N, double input[], double output[] )
     480                 : {
     481                 :     // Receives an array of dimension NxN as input.  This is passed as a one-
     482                 :     // dimensional array of N-squared size.  It produces the inverse of the
     483                 :     // input matrix, returned as output, also of size N-squared.  The Gauss-
     484                 :     // Jordan Elimination method is used.  (Adapted from a BASIC routine in
     485                 :     // "Basic Scientific Subroutines Vol. 1", courtesy of Scott Edwards.)
     486                 :   
     487                 :     // Array elements 0...N-1 are for the first row, N...2N-1 are for the
     488                 :     // second row, etc.
     489                 :   
     490                 :     // We need to have a temporary array of size N x 2N.  We'll refer to the
     491                 :     // "left" and "right" halves of this array.
     492                 :   
     493                 :     int row, col;
     494                 :   
     495                 : #if 0
     496                 :     fprintf(stderr, "Matrix Inversion input matrix (N=%d)\n", N);
     497                 :     for ( row=0; row<N; row++ )
     498                 :     {
     499                 :         for ( col=0; col<N; col++ )
     500                 :         {
     501                 :             fprintf(stderr, "%5.2f ", input[row*N + col ]  );
     502                 :         }
     503                 :         fprintf(stderr, "\n");
     504                 :     }
     505                 : #endif
     506                 :   
     507                 :     int tempSize = 2 * N * N;
     508                 :     double* temp = (double*) new double[ tempSize ];
     509                 :     double ftemp;
     510                 :   
     511                 :     if (temp == 0) {
     512                 :     
     513                 :         fprintf(stderr, "matrixInvert(): ERROR - memory allocation failed.\n");
     514                 :         return false;
     515                 :     }
     516                 :   
     517                 :     // First create a double-width matrix with the input array on the left
     518                 :     // and the identity matrix on the right.
     519                 :   
     520                 :     for ( row=0; row<N; row++ )
     521                 :     {
     522                 :         for ( col=0; col<N; col++ )
     523                 :         {
     524                 :             // Our index into the temp array is X2 because it's twice as wide
     525                 :             // as the input matrix.
     526                 :       
     527                 :             temp[ 2*row*N + col ] = input[ row*N+col ]; // left = input matrix
     528                 :             temp[ 2*row*N + col + N ] = 0.0f;     // right = 0
     529                 :         }
     530                 :         temp[ 2*row*N + row + N ] = 1.0f;   // 1 on the diagonal of RHS
     531                 :     }
     532                 :   
     533                 :     // Now perform row-oriented operations to convert the left hand side
     534                 :     // of temp to the identity matrix.  The inverse of input will then be
     535                 :     // on the right.
     536                 :   
     537                 :     int max;
     538                 :     int k=0;
     539                 :     for (k = 0; k < N; k++)
     540                 :     {
     541                 :         if (k+1 < N) // if not on the last row
     542                 :         {              
     543                 :             max = k;
     544                 :             for (row = k+1; row < N; row++) // find the maximum element
     545                 :             {  
     546                 :                 if (fabs( temp[row*2*N + k] ) > fabs( temp[max*2*N + k] ))
     547                 :                 {
     548                 :                     max = row;
     549                 :                 }
     550                 :             }
     551                 :       
     552                 :             if (max != k) // swap all the elements in the two rows
     553                 :             {        
     554                 :                 for (col=k; col<2*N; col++)
     555                 :                 {
     556                 :                     ftemp = temp[k*2*N + col];
     557                 :                     temp[k*2*N + col] = temp[max*2*N + col];
     558                 :                     temp[max*2*N + col] = ftemp;
     559                 :                 }
     560                 :             }
     561                 :         }
     562                 :     
     563                 :         ftemp = temp[ k*2*N + k ];
     564                 :         if ( ftemp == 0.0f ) // matrix cannot be inverted
     565                 :         {
     566                 :             delete[] temp;
     567                 :             return false;
     568                 :         }
     569                 :     
     570                 :         for ( col=k; col<2*N; col++ )
     571                 :         {
     572                 :             temp[ k*2*N + col ] /= ftemp;
     573                 :         }
     574                 :     
     575                 :         int i2 = k*2*N ;
     576                 :         for ( row=0; row<N; row++ )
     577                 :         {
     578                 :             if ( row != k )
     579                 :             {
     580                 :                 int i1 = row*2*N;
     581                 :                 ftemp = temp[ i1 + k ];
     582                 :                 for ( col=k; col<2*N; col++ ) 
     583                 :                 {
     584                 :                     temp[ i1 + col ] -= ftemp * temp[ i2 + col ];
     585                 :                 }
     586                 :             }
     587                 :         }
     588                 :     }
     589                 :   
     590                 :     // Retrieve inverse from the right side of temp
     591                 :   
     592                 :     for (row = 0; row < N; row++)
     593                 :     {
     594                 :         for (col = 0; col < N; col++)
     595                 :         {
     596                 :             output[row*N + col] = temp[row*2*N + col + N ];
     597                 :         }
     598                 :     }
     599                 :   
     600                 : #if 0
     601                 :     fprintf(stderr, "Matrix Inversion result matrix:\n");
     602                 :     for ( row=0; row<N; row++ )
     603                 :     {
     604                 :         for ( col=0; col<N; col++ )
     605                 :         {
     606                 :             fprintf(stderr, "%5.2f ", output[row*N + col ]  );
     607                 :         }
     608                 :         fprintf(stderr, "\n");
     609                 :     }
     610                 : #endif
     611                 :   
     612                 :     delete [] temp;       // free memory
     613                 :     return true;
     614                 : }
     615                 : #endif

Generated by: LCOV version 1.7