Main Page   Namespace List   Class Hierarchy   Alphabetical List   Compound List   File List   Namespace Members   Compound Members   File Members  

guma_lineq.cpp

Go to the documentation of this file.
00001 /* LIBGUL - Geometry Utility Library
00002  * Copyright (C) 1998-1999 Norbert Irmer
00003  *
00004  * This library is free software; you can redistribute it and/or
00005  * modify it under the terms of the GNU Library General Public
00006  * License as published by the Free Software Foundation; either
00007  * version 2 of the License, or (at your option) any later version.
00008  *
00009  * This library is distributed in the hope that it will be useful,
00010  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00011  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00012  * Library General Public License for more details.
00013  *
00014  * You should have received a copy of the GNU Library General Public
00015  * License along with this library; if not, write to the
00016  * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
00017  * Boston, MA 02111-1307, USA.
00018  */
00019 
00020 /************************************************************************      
00021   Functions for solving linear equations (see "Numerical Recipes in C")
00022 *************************************************************************/
00023 
00024  
00025 #include "stdafx.h"
00026 
00027 #include <iostream>
00028 
00029 #include "gul_types.h"
00030 #include "gul_float.h"
00031 #include "gul_vector.h"
00032 #include "guma_lineq.h"
00033 
00034 namespace guma {
00035 
00036 using gul::Ptr;
00037 using gul::rtr;
00038 using gul::SingularMatrixError;
00039 using gul::InternalError;
00040 using gul::Swap;
00041 using gul::Square;
00042 using gul::Sign;
00043 using gul::Max;
00044 using gul::Min;
00045 using gul::point;
00046 using gul::rel_equal;
00047 
00048 /*----------------------------------------------------------------*//**
00049   Solve systems of linear equations with Gauss Jordan Elimination.
00050 
00051     (A)nn * (b')nm = (b)nm
00052     (A)nn * (A')nn = (1)nn 
00053 
00054   After that, matrix 'A' contains the inverse matrix of 'A',
00055   and the columns of 'b' contain the solution vectors of the
00056   different right sides in 'b'. (see "Numerical Recipes in C")       */
00057 /*-------------------------------------------------------------------*/
00058 template< class T >
00059 GULAPI bool GaussJordan( int nRowsCols, int nRightSides, 
00060                          Ptr< Ptr<T> >& A, Ptr< Ptr<T> > &b )
00061 {
00062   Ptr<int> indexc,indexr;
00063   Ptr<T> ipivot;
00064   int icol,irow, n,i,j,k,l,ll,m;
00065   T big,pivotinv,dummy;
00066 
00067   n = nRowsCols - 1;
00068   m = nRightSides - 1;
00069 
00070   indexc.reserve_pool(n+1);
00071   indexr.reserve_pool(n+1);
00072   ipivot.reserve_pool(n+1);
00073   
00074   for( j = 0; j <= n; j++ )
00075     ipivot[j] = 0;
00076  
00077   for( i = 0; i <= n; i++ )
00078   {
00079     big = 0.0;
00080     
00081     for( j = 0; j <= n; j++ )
00082     {
00083       if( ipivot[j] != 1 )
00084       {
00085         for( k = 0; k <= n; k++ )
00086         {
00087           if( ipivot[k] == 0 )
00088           {
00089             if( rtr<T>::fabs( A[j][k] ) >= big )
00090             {
00091               big = rtr<T>::fabs( A[j][k] );
00092               irow = j;
00093               icol = k;
00094             }
00095           }
00096           else if( ipivot[k] > 1 )
00097             return false;
00098         }
00099       }
00100     } 
00101     ipivot[icol]++;
00102 
00103     if( irow != icol )
00104     {
00105       for( l = 0; l <= n; l++ )
00106         Swap( A[irow][l], A[icol][l] );
00107       for( l = 0; l <= m; l++ )
00108         Swap( b[irow][l], b[icol][l] ); 
00109     }
00110     indexr[i] = irow;
00111     indexc[i] = icol;
00112 
00113     if( A[icol][icol] == 0.0 )
00114       return false;
00115 
00116     pivotinv = (T)1 / A[icol][icol];
00117     A[icol][icol] = 1.0;
00118     for( l = 0; l <= n; l++ )
00119       A[icol][l] *= pivotinv;
00120     for( l = 0; l <= m; l++ )
00121       b[icol][l] *= pivotinv;
00122       
00123     for( ll = 0; ll <= n; ll++ )
00124     {
00125       if( ll != icol )
00126       {
00127         dummy = A[ll][icol];
00128         A[ll][icol] = 0.0;
00129         for( l = 0; l <= n; l++ )
00130           A[ll][l] -= A[icol][l] * dummy;
00131         for( l = 0; l <= m; l++ )
00132           b[ll][l] -= b[icol][l] * dummy;  
00133       }
00134     }
00135   }
00136   for( l = n; l >= 0; l-- )
00137   {
00138     if( indexr[l] != indexc[l] )
00139     {
00140       for( k = 0; k <= n; k++ )
00141         Swap( A[k][indexr[l]], A[k][indexc[l]] );
00142     }
00143   }
00144   return true;
00145 }
00146 template GULAPI bool GaussJordan( int nRowsCols, int m, 
00147                          Ptr< Ptr<float> >& A, Ptr< Ptr<float> > &b );
00148 template GULAPI bool GaussJordan( int nRowsCols, int m, 
00149                        Ptr< Ptr<double> >& A, Ptr< Ptr<double> > &b );
00150 
00151 /*-----------------------------------------------------------------------*//**
00152   Decompose a matrix into lower and upper triangular matrix.                */
00153 /*--------------------------------------------------------------------------*/
00154 template< class T >
00155 GULAPI bool LUDecomposition( int nRowsCols, Ptr< Ptr<T> >& A,
00156                              int *perm_sign, Ptr<int>& index )
00157 {
00158   Ptr<T> vv;
00159   T big,sum,dummy,temp;
00160   int i,j,k,imax,n,d;
00161 
00162   n = nRowsCols - 1;
00163   vv.reserve_pool(n+1); /* vv will contain pivot scale factor for each row */
00164   d = 1;
00165   
00166   for( i = 0; i <= n; i++ )
00167   {
00168     big = 0.0;
00169     for( j = 0; j <= n; j++ )
00170     {
00171       if( (temp = rtr<T>::fabs(A[i][j])) > big )
00172         big = temp;
00173     }    
00174     if( big == 0.0 )  
00175       return false;
00176 
00177     vv[i] = (T)1 / big;
00178   }  
00179   for( j = 0; j <= n; j++ )    /* loop over columns */
00180   {
00181     for( i = 0; i < j; i++ )
00182     {
00183       sum = A[i][j];
00184       for( k = 0; k < i; k++ )
00185         sum -= A[i][k] * A[k][j];
00186       A[i][j] = sum;
00187     }
00188     big = 0.0;
00189     
00190     for( i = j; i <= n; i++ )
00191     {
00192       sum = A[i][j];
00193       for( k = 0; k < j; k++ )
00194         sum -= A[i][k] * A[k][j];
00195       A[i][j] = sum;
00196       
00197       if( (dummy = vv[i] * rtr<T>::fabs(sum)) >= big ) // search pivot element
00198       {
00199         big = dummy;
00200         imax = i;
00201       }
00202     }   
00203     if( j != imax ) /* pivot element not on diagonal */
00204     {
00205       for( k = 0; k <= n; k++ )
00206         Swap( A[imax][k], A[j][k] );  /* swap rows */
00207  
00208       d = -d;  
00209       vv[imax] = vv[j];  /* swap scale factors */
00210     }
00211     index[j] = imax;
00212  
00213     if( A[j][j] == 0.0 )
00214       A[j][j] = rtr<T>::tiny();
00215       
00216     if( j != n )          /* divide  by pivot element */
00217     {
00218       dummy = (T)1 / A[j][j];
00219       for( i = j+1; i <= n; i++ )
00220         A[i][j] *= dummy;
00221     }  
00222   }    /* next column */ 
00223 
00224   *perm_sign = d;
00225   return true;
00226 }                                          
00227 template GULAPI bool LUDecomposition( int nRowsCols, Ptr< Ptr<float> >& A,
00228                                int *perm_sign, Ptr<int>& index );
00229 template GULAPI bool LUDecomposition( int nRowsCols, Ptr< Ptr<double> >& A,
00230                                int *perm_sign, Ptr<int>& index );
00231 
00232 /*---------------------------------------------------------------*//**
00233   Solve system of linear equations with help of LU-Decomposition
00234   (given in A).                                                     */
00235 /*------------------------------------------------------------------*/
00236 template< class T >
00237 GULAPI void LUBackSubstitution( int nRowsCols, Ptr<int>& index, 
00238                                 Ptr< Ptr<T> >& A, Ptr<T>& b )  
00239 {
00240   int ii,i,j,ip,n;
00241   T sum;
00242   
00243   n = nRowsCols-1;
00244 
00245   ii = -1;
00246 
00247   for( i = 0; i <= n; i++ )
00248   {
00249     ip = index[i];
00250     sum = b[ip];
00251     b[ip] = b[i];
00252     
00253     if( ii >= 0 )
00254     {
00255       for( j = ii; j <= i-1; j++ )
00256         sum -= A[i][j] * b[j];
00257     }
00258     else 
00259       if( sum != 0.0 )
00260         ii = i;
00261 
00262     b[i] = sum;    
00263   }
00264   for( i = n; i >= 0; i-- )
00265   {
00266     sum = b[i];
00267     
00268     for( j = i+1; j <= n; j++ )
00269       sum -= A[i][j] * b[j];
00270       
00271     b[i] = sum / A[i][i];  
00272   }
00273 }
00274 template GULAPI void LUBackSubstitution( int nRowsCols, Ptr<int>& index, 
00275                                   Ptr< Ptr<float> >& A, Ptr<float>& b ); 
00276 template GULAPI void LUBackSubstitution( int nRowsCols, Ptr<int>& index, 
00277                                   Ptr< Ptr<double> >& A, Ptr<double>& b ); 
00278 
00279 /*-------------------------------------------------------------------------*//**
00280   Multiply a band matrix and a vector: b = A*x, m1 = number of subdiagonal
00281   elements, m2 = number of superdiagonal elements, n = number of rows.
00282   It demonstrates the indexing scheme for band matrices                       */
00283 /*----------------------------------------------------------------------------*/
00284 
00285 template< class T >
00286 GULAPI void BandMVProduct( int n, int m1, int m2, 
00287                            Ptr< Ptr<T> > a, Ptr<T> x, Ptr<T> b )
00288 {
00289   int i,k,j,m;
00290 
00291   for( i = 0; i < n; i++ )
00292   {
00293     k = i - m1;
00294 
00295     m = Min(m1+m2+1,n-k);
00296 
00297     b[i] = 0;
00298 
00299     for( j = Max(0,-k); j < m; j++ )
00300       b[i] += a[i][j] * x[j+k];
00301   }
00302 }
00303 // template instantiation
00304 template GULAPI void BandMVProduct( 
00305                              int n, int m1, int m2, Ptr< Ptr<float> > a, 
00306                              Ptr<float> x, Ptr<float> b );
00307 template GULAPI void BandMVProduct( 
00308                             int n, int m1, int m2, Ptr< Ptr<double> > a, 
00309                             Ptr<double> x, Ptr<double> b );
00310 
00311 /*-----------------------------------------------------------------------*//**
00312   Decompose a band matrix into lower and upper triangular matrix: A = L*U.
00313   m1 = number of subdiagonal elements, m2 = number of superdiagonal elements,
00314   n  = number of rows and columns. L is returned in L[0..n-1][0..m1-1],
00315   U in the input matrix A; the rowwise permutation from pivoting is returned
00316   in 'index', its sign in 'perm_sign'.                                      */
00317 /*--------------------------------------------------------------------------*/
00318 
00319 template< class T >
00320 GULAPI void BandDecomposition( 
00321                         int n, int m1, int m2, Ptr< Ptr<T> >& A,
00322                         Ptr< Ptr<T> >& L, int *perm_sign, Ptr<int>& index )
00323 {
00324   int i,j,k,l,mm,d;
00325   T dum;
00326 
00327   mm = m1 + m2 + 1;
00328   l = m1;
00329   for( i = 0; i < m1; i++ )
00330   {
00331     for( j = m1-i; j < mm; j++ )
00332       A[i][j-l] = A[i][j];
00333     l--;
00334     for( j = mm-l-1; j < mm; j++ )
00335       A[i][j] = 0.0;
00336   }
00337   d = 1;
00338   l = m1;
00339 
00340   for( k = 0; k < n; k++ )
00341   {
00342     dum = A[k][0];
00343     i = k;
00344 
00345     if( l < n ) l++;
00346     for( j = k+1; j < l; j++ )
00347     {
00348       if( rtr<T>::fabs(A[j][0]) > rtr<T>::fabs(dum) )
00349       {
00350         dum = A[j][0];
00351         i = j;
00352       }
00353     }
00354     index[k] = i;
00355 
00356     if( dum == 0.0 )     /* if algorithmically singular */
00357       A[k][0] = rtr<T>::tiny();
00358 
00359     if( i != k )         /* interchange rows */
00360     {
00361       d = -d;
00362       for( j = 0; j < mm; j++ )
00363         Swap( A[k][j], A[i][j] );
00364     }
00365     
00366     for( i = k+1; i < l; i++ )  // i = i'+1
00367     {
00368       dum = A[i][0]/A[k][0];
00369       L[k][i-k-1] = dum;
00370 
00371       for( j = 1; j < mm; j++ )
00372         A[i][j-1] = A[i][j] - dum*A[k][j];
00373 
00374       A[i][mm-1] = 0.0;
00375     }
00376   }
00377   *perm_sign = d;
00378 }
00379 // template instantiation
00380 template GULAPI void BandDecomposition( 
00381              int n, int m1, int m2, Ptr< Ptr<float> >& A, Ptr< Ptr<float> >& L,
00382              int *perm_sign, Ptr<int>& index );
00383 template GULAPI void BandDecomposition( 
00384            int n, int m1, int m2, Ptr< Ptr<double> >& A, Ptr< Ptr<double> >& L,
00385            int *perm_sign, Ptr<int>& index );
00386 
00387 /*-----------------------------------------------------------------------*//**
00388   Solve system of linear equations with help of LU-Decomposition of
00389   a band matrix (given by L and A).                                                     */
00390 /*--------------------------------------------------------------------------*/
00391 
00392 template< class T >
00393 GULAPI void BandBackSubstitution( 
00394                   int n, int m1, int m2, const Ptr< Ptr<T> >& A,
00395                   const Ptr< Ptr<T> >& L, const Ptr<int>& index, Ptr<T>& b )
00396 {
00397   int i,k,l,mm;
00398   T dum;
00399 
00400   mm = m1 + m2 + 1;
00401   l = m1;
00402 
00403   for( k = 0; k < n; k++ ) /* Forward substitution */
00404   {
00405     i = index[k];
00406     if( i != k ) Swap( b[k], b[i] );
00407     if( l < n ) l++;
00408     for( i = k+1; i < l; i++ )
00409       b[i] -= L[k][i-k-1]*b[k];
00410   }
00411   l = 1;
00412   for( i = n-1; i >= 0; i-- ) /* Forward substitution */
00413   {
00414     dum = b[i];
00415     for( k = 1; k < l; k++ )
00416       dum -= A[i][k]*b[k+i];
00417     b[i] = dum/A[i][0];
00418     if( l < mm ) l++;
00419   }
00420 }
00421 // template instantiation
00422 template GULAPI void BandBackSubstitution( 
00423            int n, int m1, int m2, const Ptr< Ptr<float> >& A, 
00424            const Ptr< Ptr<float> >& L, const Ptr<int>& index, Ptr<float>& b );
00425 template GULAPI void BandBackSubstitution( 
00426         int n, int m1, int m2, const Ptr< Ptr<double> >& A, 
00427         const Ptr< Ptr<double> >& L, const Ptr<int>& index, Ptr<double>& b );
00428 
00429 
00430 /*------------------------------------------------------------------------*//**
00431   Singular Value Decomposition.
00432   =============================
00433 
00434   A = U * W * V^{T} with
00435 
00436     A = m x n Matrix     (m >= n)
00437     U = m x n Matrix
00438     W = n x n Matrix
00439     V = n x n Matrix
00440         
00441     U,V orthogonal ( i.e. U^{T} * U = (1), V^{T} * V = (1) )
00442     W diagonal matrix, containing the singular values.                       */
00443 /*---------------------------------------------------------------------------*/
00444 template< class T >
00445 GULAPI void SVDecomposition( int m, int n, Ptr< Ptr<T> >& a,
00446                           Ptr<T>& w, Ptr< Ptr<T> >& v )
00447 {
00448   int flag,i,its,j,jj,k,l,nm;
00449   T anorm,c,f,g,h,s,scale,x,y,z;
00450   Ptr<T> rv1;
00451 
00452   rv1.reserve_pool(n+1);
00453 
00454 /* ----- Householder reduction to bidiagonal form ------------------- */
00455 
00456   g = scale = anorm = 0.0;
00457   
00458   for( i = 1; i <= n; i++ )
00459   {
00460     l = i+1;
00461     rv1[i-1] = scale * g;
00462     g = s = scale = 0.0;
00463     
00464     if( i <= m )
00465     {
00466       for( k = i; k <=m; k++ )
00467         scale += rtr<T>::fabs( a[k-1][i-1] );
00468       if( scale != 0.0 )
00469       {
00470         for( k = i; k <= m; k++ )
00471         {
00472           a[k-1][i-1] /= scale;
00473           s += a[k-1][i-1] * a[k-1][i-1];
00474         }
00475         f = a[i-1][i-1];
00476         g = -Sign( rtr<T>::sqrt(s), f );
00477         h = f*g - s;
00478         a[i-1][i-1] = f-g;
00479         
00480         for( j=l; j<=n; j++ )
00481         {
00482           for( s=0.0,k=i; k<=m; k++ )
00483             s += a[k-1][i-1] * a[k-1][j-1];
00484           f = s / h;
00485           for( k = i; k <= m; k++ )
00486             a[k-1][j-1] += f * a[k-1][i-1];  
00487         }    
00488         for( k = i; k <=m; k++ )
00489           a[k-1][i-1] *= scale;
00490       }
00491     }
00492     w[i-1] = scale * g;
00493     g = s = scale = 0.0;
00494     
00495     if( (i <= m) && (i != n) )
00496     {
00497       for( k = l; k <= n; k++ )
00498         scale += rtr<T>::fabs( a[i-1][k-1] );
00499         
00500       if( scale != 0.0 )
00501       {  
00502         for( k = l; k <= n; k++ )
00503         {
00504           a[i-1][k-1] /= scale;
00505           s += a[i-1][k-1] * a[i-1][k-1];
00506         }
00507 
00508         f = a[i-1][l-1];
00509         g = -Sign( rtr<T>::sqrt(s), f );
00510         h = f*g - s;
00511         a[i-1][l-1] = f-g;
00512         
00513         for( k = l; k <= n; k++ )
00514           rv1[k-1] = a[i-1][k-1] / h;
00515           
00516         for( j = l; j <= m; j++ )
00517         {
00518           for( s = 0.0,k=l; k <= n; k++ )
00519             s += a[j-1][k-1] * a[i-1][k-1];
00520           for( k = l; k <= n; k++ )
00521             a[j-1][k-1] += s * rv1[k-1];
00522         }      
00523         
00524         for( k = l; k <= n; k++ )
00525           a[i-1][k-1] *= scale;          
00526       }  
00527     }
00528     anorm = Max( anorm, (rtr<T>::fabs(w[i-1]) + rtr<T>::fabs(rv1[i-1])) );
00529   }  
00530  /* ------- akkumulation of right side transformations ------------- */
00531 
00532   for( i = n; i >= 1; i-- ) 
00533   {
00534     if( i < n )
00535     {
00536       if( g != 0.0 )
00537       {
00538         for( j = l; j <= n; j++ )
00539           v[j-1][i-1] = (a[i-1][j-1] / a[i-1][l-1]) / g;
00540         for( j = l; j <= n; j++ )
00541         {
00542           for( s=0.0,k=l; k<=n; k++ )
00543             s += a[i-1][k-1] * v[k-1][j-1];
00544           for( k = l; k <= n; k++ )
00545             v[k-1][j-1] += s * v[k-1][i-1];
00546         }          
00547       }
00548       for( j = l; j <= n; j++ )
00549         v[i-1][j-1] = v[j-1][i-1] = 0.0;
00550     }
00551     v[i-1][i-1] = 1.0;
00552     g = rv1[i-1];
00553     l = i;
00554   }    
00555 /* -------- accumulation of left side transformations --------------- */
00556 
00557   for( i = Min(m,n); i >=1; i-- ) 
00558   {
00559     l = i+1;
00560     g = w[i-1];
00561     
00562     for( j = l; j <= n; j++ )
00563       a[i-1][j-1] = 0.0;
00564       
00565     if( g != 0.0 )
00566     {
00567       g = (T)1 / g;
00568       
00569       for( j = l; j <= n; j++ )
00570       {
00571         for( s=0.0,k=l; k <= m; k++ )
00572           s += a[k-1][i-1] * a[k-1][j-1];
00573         
00574         f = (s/a[i-1][i-1]) * g;
00575 
00576         for( k = i; k <= m; k++ )
00577           a[k-1][j-1] += f * a[k-1][i-1];
00578       }
00579 
00580       for( j = i; j <= m; j++ )
00581         a[j-1][i-1] *= g;      
00582     }  
00583     else
00584       for( j = i; j <= m; j++ )
00585         a[j-1][i-1] = 0.0;
00586         
00587     a[i-1][i-1] += 1.0;    
00588   }        
00589 /* -------- Diagonalize the bidiagonal form --------------------- */
00590     
00591   for( k = n; k >= 1; k-- )      
00592   {
00593     for( its = 1; its <= 30; its++ )
00594     {
00595       flag = 1;
00596       
00597       for( l = k; l >= 1; l-- )
00598       {
00599         nm = l-1;
00600         if( rtr<T>::fabs(rv1[l-1]) + anorm == anorm )
00601         {
00602           flag = 0;
00603           break;
00604         }  
00605         if( rtr<T>::fabs(w[nm-1]) + anorm == anorm )
00606           break;
00607       } 
00608       if( flag != 0 )
00609       {
00610         c = 0.0;
00611         s = 1.0;
00612         for( i = l; i <= k; i++ )
00613         {
00614           f = s * rv1[i-1];
00615           rv1[i-1] = c * rv1[i-1];
00616           
00617           if( rtr<T>::fabs(f) + anorm == anorm )
00618             break;
00619             
00620           g = w[i-1];
00621           h = Pythagoras( f, g );
00622           w[i-1] = h;
00623           h = (T)1 / h;
00624           c = g * h;
00625           s = -f * h;
00626           
00627           for( j = 1; j <= m; j++ )
00628           {
00629             y = a[j-1][nm-1];
00630             z = a[j-1][i-1];
00631             a[j-1][nm-1] = y * c + z * s;
00632             a[j-1][i-1] = z * c - y * s;
00633           }
00634         }
00635       }
00636       z = w[k-1];
00637       if( l == k )
00638       {
00639         if( z < 0.0 )
00640         {
00641           w[k-1] = -z;
00642           for( j = 1; j <= n; j++ )
00643             v[j-1][k-1] = -v[j-1][k-1];
00644         }
00645         break;
00646       }              
00647       if( its == 30 )
00648         throw InternalError();
00649       
00650       x = w[l-1];
00651       nm = k-1;
00652       y = w[nm-1];
00653       g = rv1[nm-1];
00654       h = rv1[k-1];
00655       f = ((y-z)*(y+z)+(g-h)*(g+h))/((T)2*h*y);
00656       g = Pythagoras( f, (T)1 );
00657       f = ((x-z)*(x+z) + h*((y/(f+Sign(g,f)))-h)) / x;
00658       c = s = 1.0;
00659       
00660       for( j = l; j <= nm; j++ )
00661       {
00662         i = j+1;
00663         g = rv1[i-1];
00664         y = w[i-1];
00665         h = s * g;
00666         g = c * g;
00667         z = Pythagoras( f, h );
00668         rv1[j-1] = z;
00669         c = f/z;
00670         s = h/z;
00671         f = x*c + g*s;
00672         g = g*c - x*s;
00673         h = y*s;
00674         y *= c;
00675         
00676         for( jj = 1; jj <= n; jj++ )
00677         {
00678           x = v[jj-1][j-1];
00679           z = v[jj-1][i-1];
00680           v[jj-1][j-1] = x*c + z*s;
00681           v[jj-1][i-1] = z*c - x*s;
00682         }
00683         z = Pythagoras( f, h );
00684         w[j-1] = z;
00685         if( z != 0.0 )
00686         {
00687           z = (T)1 / z;
00688           c = f*z;
00689           s = h*z;
00690         }
00691         f = c*g + s*y;
00692         x = c*y - s*g;
00693         
00694         for( jj = 1; jj <= m; jj++ )
00695         {
00696           y = a[jj-1][j-1];
00697           z = a[jj-1][i-1];
00698           a[jj-1][j-1] = y*c + z*s;
00699           a[jj-1][i-1] = z*c - y*s;
00700         }
00701       }
00702       rv1[l-1] = 0.0;
00703       rv1[k-1] = f;
00704       w[k-1] = x;
00705     }
00706   }
00707 }    
00708 template GULAPI void SVDecomposition( int m, int n, Ptr< Ptr<float> >& a,
00709                                    Ptr<float>& w, Ptr< Ptr<float> >& v );
00710 template GULAPI void SVDecomposition( int m, int n, Ptr< Ptr<double> >& a,
00711                                    Ptr<double>& w, Ptr< Ptr<double> >& v );
00712 
00713 /*-------------------------------------------------------------------*//**
00714   Solve system of linear equations, when a Singular Value Decomposition
00715   is given                                                              */
00716 /*----------------------------------------------------------------------*/  
00717 template< class T >            
00718 GULAPI void SVBackSubstitution( int m, int n,
00719                          Ptr< Ptr<T> >& u, Ptr<T>& w, 
00720                          Ptr< Ptr<T> >& v, Ptr<T>& b,
00721                          Ptr<T>& x ) 
00722 {
00723   int jj,j,i;
00724   T s;
00725   Ptr<T> tmp;
00726   
00727   tmp.reserve_pool(n);
00728   
00729   for( j = 1; j <= n; j++ )
00730   {
00731     s = 0.0;
00732     if( w[j-1] != 0.0 )
00733     {
00734       for( i = 1; i <= m; i++ )
00735         s += u[i-1][j-1] * b[i-1];
00736        s /= w[j-1];
00737     }
00738     tmp[j-1] = s;
00739   }  
00740   for( j = 1; j <= n; j++ )
00741   {
00742     s = 0.0;
00743     for ( jj = 1; jj <= n; jj++ )
00744       s += v[j-1][jj-1] * tmp[jj-1];
00745     x[j-1] = s;
00746   }      
00747   return;
00748 }          
00749 template GULAPI void SVBackSubstitution( int m, int n,
00750                          Ptr< Ptr<float> >& u, Ptr<float>& w, 
00751                          Ptr< Ptr<float> >& v, Ptr<float>& b,
00752                          Ptr<float>& x );
00753 template GULAPI void SVBackSubstitution( int m, int n,
00754                          Ptr< Ptr<double> >& u, Ptr<double>& w, 
00755                          Ptr< Ptr<double> >& v, Ptr<double>& b,
00756                          Ptr<double>& x );
00757 
00758 
00759 /*-------------------------------------------------------------------*//**
00760   Solve system of linear equations, when a Singular Value Decomposition
00761   is given                                                              */
00762 /*----------------------------------------------------------------------*/  
00763 template< class T >            
00764 GULAPI void SVBackSubstitution( int m, int n,
00765                          Ptr< Ptr<T> >& u, Ptr<T>& w, 
00766                          Ptr< Ptr<T> >& v, Ptr<T>& b ) 
00767 {
00768   int jj,j,i;
00769   T s;
00770   Ptr<T> tmp;
00771   
00772   tmp.reserve_pool(n);
00773   
00774   for( j = 1; j <= n; j++ )
00775   {
00776     s = 0.0;
00777     if( w[j-1] != 0.0 )
00778     {
00779       for( i = 1; i <= m; i++ )
00780         s += u[i-1][j-1] * b[i-1];
00781        s /= w[j-1];
00782     }
00783     tmp[j-1] = s;
00784   }  
00785   for( j = 1; j <= n; j++ )
00786   {
00787     s = 0.0;
00788     for ( jj = 1; jj <= n; jj++ )
00789       s += v[j-1][jj-1] * tmp[jj-1];
00790     b[j-1] = s;
00791   }      
00792   return;
00793 }          
00794 template GULAPI void SVBackSubstitution( int m, int n,
00795                          Ptr< Ptr<float> >& u, Ptr<float>& w, 
00796                          Ptr< Ptr<float> >& v, Ptr<float>& b );
00797 template GULAPI void SVBackSubstitution( int m, int n,
00798                          Ptr< Ptr<double> >& u, Ptr<double>& w, 
00799                          Ptr< Ptr<double> >& v, Ptr<double>& b );
00800 
00801 /*---------------------------------------------------------------------*//**
00802   Sets very small singular values to zero. This avoids disastrous
00803   rounding errors, when the coefficient matrix of a system of
00804   linear equations is ill-conditioned.
00805   If some singular values were zero, or had to be set to zero, the function
00806   returns false, else it returns true                                     */
00807 /*------------------------------------------------------------------------*/
00808 template< class T >
00809 GULAPI bool SVZero( int n, Ptr<T>& w )
00810 {
00811   T wmax,wmin;
00812   int j;
00813   bool well_conditioned;
00814   T limit = (T)n * rtr<T>::epsilon();
00815 
00816   wmax = wmin = rtr<T>::fabs(w[0]);
00817   for( j = 2; j <= n; j++ )
00818   {
00819     if( rtr<T>::fabs(w[j-1]) > wmax ) wmax = rtr<T>::fabs(w[j-1]);
00820     else if( rtr<T>::fabs(w[j-1]) < wmin ) wmin = rtr<T>::fabs(w[j-1]);
00821   }
00822   // ill conditioned ?
00823   if( (wmax == 0.0) ||
00824       (wmin / wmax < limit) )
00825     well_conditioned = false;   
00826   else
00827     well_conditioned = true;
00828 
00829   /*
00830   cout << "wmin/wmax = " << wmin / wmax << "\n";
00831   */
00832                                     
00833   wmin = wmax * limit;
00834   
00835   for( j = 1; j <= n; j++ )
00836     if( wmin > rtr<T>::fabs(w[j-1]) ) w[j-1] = 0.0;
00837 
00838   return well_conditioned;
00839 }    
00840 template GULAPI bool SVZero( int n, Ptr<float>& w );
00841 template GULAPI bool SVZero( int n, Ptr<double>& w );
00842 
00843 
00844 /*-----------------------------------------------------------------------*//**
00845   Intersect two lines in 3-dimensions, using singular decomposition.
00846   (remark: this is a overkill, if you alredy know that the two lines 
00847   intersect (but on the other hand this is the first oppurtinity
00848   to do something senseful with the above SVD algorithm:)))))
00849 
00850   retcode = 0  => intersection point found
00851   retcode = 1  => lines are parallel
00852   retcode = 2  => lines are not parallel, but don't intersect either
00853                   (what's the english word for "windschief" ?)
00854   In the last two cases the solution minimizes |x|^2 or |Ax + b|,
00855   and the midst of the two points on the two lines is returned.              */
00856 /*---------------------------------------------------------------------------*/
00857 template< class T >
00858 int SVDIntersectLines( point<T> P1, point<T> T1,
00859                        point<T> P2, point<T> T2,
00860                        T *alpha1, T *alpha2, point<T> *P )
00861 {
00862   Ptr< Ptr<T> > u,v; 
00863   Ptr<T> w, x, b;
00864   point<T> L1,L2;
00865   int code,i;
00866 
00867   u.reserve_pool(3);
00868   for( i = 0; i < 3; i++ )
00869     u[i].reserve_pool(2);
00870   v.reserve_pool(2);
00871   for( i = 0; i < 2; i++ )
00872     v[i].reserve_pool(2);
00873   w.reserve_pool(2);
00874   x.reserve_pool(2);
00875   b.reserve_pool(3);
00876 
00877   code = 0;
00878 
00879   u[0][0] = T1.x;  u[0][1] = -T2.x;
00880   u[1][0] = T1.y;  u[1][1] = -T2.y;
00881   u[2][0] = T1.z;  u[2][1] = -T2.z;
00882 
00883   SVDecomposition( 3, 2, u, w, v );
00884   
00885   if( SVZero( 2, w ) ) // are the lines parallel ?
00886     code = 1;  
00887     
00888   b[0] = P2.x - P1.x;
00889   b[1] = P2.y - P1.y;
00890   b[2] = P2.z - P1.z;
00891 
00892   SVBackSubstitution( 3, 2, u, w, v, b, x );
00893 
00894   L1.x = P1.x + x[0] * T1.x; 
00895   L1.y = P1.y + x[0] * T1.y; 
00896   L1.z = P1.z + x[0] * T1.z; 
00897 
00898   L2.x = P2.x + x[1] * T2.x; 
00899   L2.y = P2.y + x[1] * T2.y; 
00900   L2.z = P2.z + x[1] * T2.z; 
00901 
00902   // not parallel, but points too far away from each other ?
00903   if( !code && !rel_equal(L1, L2, rtr<T>::zero_tol() ) )
00904     code = 2;
00905 
00906   P->x = (L1.x + L2.x) / (T)2; 
00907   P->y = (L1.y + L2.y) / (T)2; 
00908   P->z = (L1.z + L2.z) / (T)2; 
00909 
00910   *alpha1 = x[0];
00911   *alpha2 = x[1];
00912 
00913   return code;
00914 }
00915 template int SVDIntersectLines( point<float> P1, point<float> T1,
00916                       point<float> P2, point<float> T2,
00917                       float *alpha1, float *alpha2, point<float> *P );
00918 template int SVDIntersectLines( point<double> P1, point<double> T1,
00919                     point<double> P2, point<double> T2,
00920                     double *alpha1, double *alpha2, point<double> *P );
00921 
00922 
00923 
00924 
00925 
00926 
00927 
00928 #if 0
00929 /*----------------------------------------------------------------------
00930 
00931 A email I found in comp.graphics.algorithms:
00932 ============================================
00933 
00934 If you have a set of points in homogenus coordinates [x y z 1]' (note that w
00935 is normalized to 1). You should calculate the sum of outer products of all
00936 the coordinates:
00937 
00938 A = sum( [x y z 1]' * [x y z 1] )
00939 
00940 That will give you a positive definite symmetric 4x4 matrix. Now calculate
00941 the eigenvector that corresponds to the smallest eigenvalue of A, and call
00942 it [a b c d]'. Now the planes equation is:
00943 
00944 0 = [a b c d] * [x y z 1]' = a*x + b*y + c*z +d
00945 
00946 This equation minimizes the sum of squared distances betwen the points and
00947 the plane (measured perpendicular to the plane). Degenerate cases where it's
00948 unclear what plane to choose, will show up as multiple small eigenvalues of A.
00949 
00950 /Henrik Gustavsson
00951 
00952 > Elie Jamaa wrote
00953 >
00954 > I'm trying to write a plug-in that collapses a cloud of  n points (n>3
00955 > of course) onto their average plane .
00956 > Does anybody know of a formula that relates the plane's equation (maybe
00957 > cartesian) to the xyz coordinates of the n points ?
00958 >
00959 > I know that there can be several roots if, for example, I have 8 points
00960 > that are at the corners of a cube, then 3 planes are valid (like the xy, yz,
00961 > xz planes).
00962 > Also, there can be an inifinity of solutions if the points are aligned.
00963 > I guess the formula would be a division by zero or something funky like
00964 > that in those cases.
00965 > But I don't intend to find out the roots : the points will just not be
00966 > flattened if there isn't only one average plane.
00967 >
00968 > Any idea ?
00969 
00970 -------------------------------------------------------------------------*/
00971 
00972 void FindPlane( int nP, const Ptr< point<T> >& P )
00973 {
00974   A.reserve_pool(4); for( i=0; i<4; i++ ) A[i].reserve_pool(4);
00975   V.reserve_pool(4); for( i=0; i<4; i++ ) V[i].reserve_pool(4);
00976   w.reserve_pool(4);
00977 
00978   for( i = 0; i < 4; i++ )
00979     for( j = 0; j < 4; j++ )
00980       A[i][j] = (T)0;
00981 
00982   for( i = 0; i < nP; i++ )
00983   {
00984     x = P[i].x; y = P[i].y; z = P[i].y;
00985 
00986     A[0][0] += x*x;  A[0][1] += x*y;  A[0][2] += x*z;  A[0][3] += x;
00987     A[1][0] += y*x;  A[1][1] += y*y;  A[1][2] += y*z;  A[1][3] += y;
00988     A[2][0] += z*x;  A[2][1] += z*y;  A[2][2] += z*z;  A[2][3] += z;
00989     A[3][0] += x;    A[3][1] += y;    A[3][2] += z;    A[3][3] += (T)1;
00990   }
00991 
00992   /* I think I will calc the Eigenvectors with Jacobi Transformation
00993      (in Numerical Recipes) */
00994 }
00995 #endif
00996 
00997 }
00998 
00999 
01000 
01001 

Generated on Mon Jan 21 04:17:34 2002 for GUL 0.6 - Geometry Utility Library by doxygen1.2.13.1 written by Dimitri van Heesch, © 1997-2001