---

guma_lineq.cpp

Go to the documentation of this file.

/* LIBGUL - Geometry Utility Library
 * Copyright (C) 1998-1999 Norbert Irmer
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Library General Public
 * License as published by the Free Software Foundation; either
 * version 2 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Library General Public License for more details.
 *
 * You should have received a copy of the GNU Library General Public
 * License along with this library; if not, write to the
 * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA.
 */

/************************************************************************      
  Functions for solving linear equations (see "Numerical Recipes in C")
*************************************************************************/

 
#include "stdafx.h"

#include <iostream>

#include "gul_types.h"
#include "gul_float.h"
#include "gul_vector.h"
#include "guma_lineq.h"

namespace guma {

using gul::Ptr;
using gul::rtr;
using gul::SingularMatrixError;
using gul::InternalError;
using gul::Swap;
using gul::Square;
using gul::Sign;
using gul::Max;
using gul::Min;
using gul::point;
using gul::rel_equal;

/*----------------------------------------------------------------*//**
  Solve systems of linear equations with Gauss Jordan Elimination.

    (A)nn * (b')nm = (b)nm
    (A)nn * (A')nn = (1)nn 

  After that, matrix 'A' contains the inverse matrix of 'A',
  and the columns of 'b' contain the solution vectors of the
  different right sides in 'b'. (see "Numerical Recipes in C")       */
/*-------------------------------------------------------------------*/
template< class T >
GULAPI bool GaussJordan( int nRowsCols, int nRightSides, 
                         Ptr< Ptr<T> >& A, Ptr< Ptr<T> > &b )
{
  Ptr<int> indexc,indexr;
  Ptr<T> ipivot;
  int icol,irow, n,i,j,k,l,ll,m;
  T big,pivotinv,dummy;

  n = nRowsCols - 1;
  m = nRightSides - 1;

  indexc.reserve_pool(n+1);
  indexr.reserve_pool(n+1);
  ipivot.reserve_pool(n+1);
  
  for( j = 0; j <= n; j++ )
    ipivot[j] = 0;
 
  for( i = 0; i <= n; i++ )
  {
    big = 0.0;
    
    for( j = 0; j <= n; j++ )
    {
      if( ipivot[j] != 1 )
      {
        for( k = 0; k <= n; k++ )
        {
          if( ipivot[k] == 0 )
          {
            if( rtr<T>::fabs( A[j][k] ) >= big )
            {
              big = rtr<T>::fabs( A[j][k] );
              irow = j;
              icol = k;
            }
          }
          else if( ipivot[k] > 1 )
            return false;
        }
      }
    } 
    ipivot[icol]++;

    if( irow != icol )
    {
      for( l = 0; l <= n; l++ )
        Swap( A[irow][l], A[icol][l] );
      for( l = 0; l <= m; l++ )
        Swap( b[irow][l], b[icol][l] ); 
    }
    indexr[i] = irow;
    indexc[i] = icol;

    if( A[icol][icol] == 0.0 )
      return false;

    pivotinv = (T)1 / A[icol][icol];
    A[icol][icol] = 1.0;
    for( l = 0; l <= n; l++ )
      A[icol][l] *= pivotinv;
    for( l = 0; l <= m; l++ )
      b[icol][l] *= pivotinv;
      
    for( ll = 0; ll <= n; ll++ )
    {
      if( ll != icol )
      {
        dummy = A[ll][icol];
        A[ll][icol] = 0.0;
        for( l = 0; l <= n; l++ )
          A[ll][l] -= A[icol][l] * dummy;
        for( l = 0; l <= m; l++ )
          b[ll][l] -= b[icol][l] * dummy;  
      }
    }
  }
  for( l = n; l >= 0; l-- )
  {
    if( indexr[l] != indexc[l] )
    {
      for( k = 0; k <= n; k++ )
        Swap( A[k][indexr[l]], A[k][indexc[l]] );
    }
  }
  return true;
}
template GULAPI bool GaussJordan( int nRowsCols, int m, 
                         Ptr< Ptr<float> >& A, Ptr< Ptr<float> > &b );
template GULAPI bool GaussJordan( int nRowsCols, int m, 
                       Ptr< Ptr<double> >& A, Ptr< Ptr<double> > &b );

/*-----------------------------------------------------------------------*//**
  Decompose a matrix into lower and upper triangular matrix.                */
/*--------------------------------------------------------------------------*/
template< class T >
GULAPI bool LUDecomposition( int nRowsCols, Ptr< Ptr<T> >& A,
                             int *perm_sign, Ptr<int>& index )
{
  Ptr<T> vv;
  T big,sum,dummy,temp;
  int i,j,k,imax,n,d;

  n = nRowsCols - 1;
  vv.reserve_pool(n+1); /* vv will contain pivot scale factor for each row */
  d = 1;
  
  for( i = 0; i <= n; i++ )
  {
    big = 0.0;
    for( j = 0; j <= n; j++ )
    {
      if( (temp = rtr<T>::fabs(A[i][j])) > big )
        big = temp;
    }    
    if( big == 0.0 )  
      return false;

    vv[i] = (T)1 / big;
  }  
  for( j = 0; j <= n; j++ )    /* loop over columns */
  {
    for( i = 0; i < j; i++ )
    {
      sum = A[i][j];
      for( k = 0; k < i; k++ )
        sum -= A[i][k] * A[k][j];
      A[i][j] = sum;
    }
    big = 0.0;
    
    for( i = j; i <= n; i++ )
    {
      sum = A[i][j];
      for( k = 0; k < j; k++ )
        sum -= A[i][k] * A[k][j];
      A[i][j] = sum;
      
      if( (dummy = vv[i] * rtr<T>::fabs(sum)) >= big ) // search pivot element
      {
        big = dummy;
        imax = i;
      }
    }   
    if( j != imax ) /* pivot element not on diagonal */
    {
      for( k = 0; k <= n; k++ )
        Swap( A[imax][k], A[j][k] );  /* swap rows */
 
      d = -d;  
      vv[imax] = vv[j];  /* swap scale factors */
    }
    index[j] = imax;
 
    if( A[j][j] == 0.0 )
      A[j][j] = rtr<T>::tiny();
      
    if( j != n )          /* divide  by pivot element */
    {
      dummy = (T)1 / A[j][j];
      for( i = j+1; i <= n; i++ )
        A[i][j] *= dummy;
    }  
  }    /* next column */ 

  *perm_sign = d;
  return true;
}                                          
template GULAPI bool LUDecomposition( int nRowsCols, Ptr< Ptr<float> >& A,
                               int *perm_sign, Ptr<int>& index );
template GULAPI bool LUDecomposition( int nRowsCols, Ptr< Ptr<double> >& A,
                               int *perm_sign, Ptr<int>& index );

/*---------------------------------------------------------------*//**
  Solve system of linear equations with help of LU-Decomposition
  (given in A).                                                     */
/*------------------------------------------------------------------*/
template< class T >
GULAPI void LUBackSubstitution( int nRowsCols, Ptr<int>& index, 
                                Ptr< Ptr<T> >& A, Ptr<T>& b )  
{
  int ii,i,j,ip,n;
  T sum;
  
  n = nRowsCols-1;

  ii = -1;

  for( i = 0; i <= n; i++ )
  {
    ip = index[i];
    sum = b[ip];
    b[ip] = b[i];
    
    if( ii >= 0 )
    {
      for( j = ii; j <= i-1; j++ )
        sum -= A[i][j] * b[j];
    }
    else 
      if( sum != 0.0 )
        ii = i;

    b[i] = sum;    
  }
  for( i = n; i >= 0; i-- )
  {
    sum = b[i];
    
    for( j = i+1; j <= n; j++ )
      sum -= A[i][j] * b[j];
      
    b[i] = sum / A[i][i];  
  }
}
template GULAPI void LUBackSubstitution( int nRowsCols, Ptr<int>& index, 
                                  Ptr< Ptr<float> >& A, Ptr<float>& b ); 
template GULAPI void LUBackSubstitution( int nRowsCols, Ptr<int>& index, 
                                  Ptr< Ptr<double> >& A, Ptr<double>& b ); 

/*-------------------------------------------------------------------------*//**
  Multiply a band matrix and a vector: b = A*x, m1 = number of subdiagonal
  elements, m2 = number of superdiagonal elements, n = number of rows.
  It demonstrates the indexing scheme for band matrices                       */
/*----------------------------------------------------------------------------*/

template< class T >
GULAPI void BandMVProduct( int n, int m1, int m2, 
                           Ptr< Ptr<T> > a, Ptr<T> x, Ptr<T> b )
{
  int i,k,j,m;

  for( i = 0; i < n; i++ )
  {
    k = i - m1;

    m = Min(m1+m2+1,n-k);

    b[i] = 0;

    for( j = Max(0,-k); j < m; j++ )
      b[i] += a[i][j] * x[j+k];
  }
}
// template instantiation
template GULAPI void BandMVProduct( 
                             int n, int m1, int m2, Ptr< Ptr<float> > a, 
                             Ptr<float> x, Ptr<float> b );
template GULAPI void BandMVProduct( 
                            int n, int m1, int m2, Ptr< Ptr<double> > a, 
                            Ptr<double> x, Ptr<double> b );

/*-----------------------------------------------------------------------*//**
  Decompose a band matrix into lower and upper triangular matrix: A = L*U.
  m1 = number of subdiagonal elements, m2 = number of superdiagonal elements,
  n  = number of rows and columns. L is returned in L[0..n-1][0..m1-1],
  U in the input matrix A; the rowwise permutation from pivoting is returned
  in 'index', its sign in 'perm_sign'.                                      */
/*--------------------------------------------------------------------------*/

template< class T >
GULAPI void BandDecomposition( 
                        int n, int m1, int m2, Ptr< Ptr<T> >& A,
                        Ptr< Ptr<T> >& L, int *perm_sign, Ptr<int>& index )
{
  int i,j,k,l,mm,d;
  T dum;

  mm = m1 + m2 + 1;
  l = m1;
  for( i = 0; i < m1; i++ )
  {
    for( j = m1-i; j < mm; j++ )
      A[i][j-l] = A[i][j];
    l--;
    for( j = mm-l-1; j < mm; j++ )
      A[i][j] = 0.0;
  }
  d = 1;
  l = m1;

  for( k = 0; k < n; k++ )
  {
    dum = A[k][0];
    i = k;

    if( l < n ) l++;
    for( j = k+1; j < l; j++ )
    {
      if( rtr<T>::fabs(A[j][0]) > rtr<T>::fabs(dum) )
      {
        dum = A[j][0];
        i = j;
      }
    }
    index[k] = i;

    if( dum == 0.0 )     /* if algorithmically singular */
      A[k][0] = rtr<T>::tiny();

    if( i != k )         /* interchange rows */
    {
      d = -d;
      for( j = 0; j < mm; j++ )
        Swap( A[k][j], A[i][j] );
    }
    
    for( i = k+1; i < l; i++ )  // i = i'+1
    {
      dum = A[i][0]/A[k][0];
      L[k][i-k-1] = dum;

      for( j = 1; j < mm; j++ )
        A[i][j-1] = A[i][j] - dum*A[k][j];

      A[i][mm-1] = 0.0;
    }
  }
  *perm_sign = d;
}
// template instantiation
template GULAPI void BandDecomposition( 
             int n, int m1, int m2, Ptr< Ptr<float> >& A, Ptr< Ptr<float> >& L,
             int *perm_sign, Ptr<int>& index );
template GULAPI void BandDecomposition( 
           int n, int m1, int m2, Ptr< Ptr<double> >& A, Ptr< Ptr<double> >& L,
           int *perm_sign, Ptr<int>& index );

/*-----------------------------------------------------------------------*//**
  Solve system of linear equations with help of LU-Decomposition of
  a band matrix (given by L and A).                                                     */
/*--------------------------------------------------------------------------*/

template< class T >
GULAPI void BandBackSubstitution( 
                  int n, int m1, int m2, const Ptr< Ptr<T> >& A,
                  const Ptr< Ptr<T> >& L, const Ptr<int>& index, Ptr<T>& b )
{
  int i,k,l,mm;
  T dum;

  mm = m1 + m2 + 1;
  l = m1;

  for( k = 0; k < n; k++ ) /* Forward substitution */
  {
    i = index[k];
    if( i != k ) Swap( b[k], b[i] );
    if( l < n ) l++;
    for( i = k+1; i < l; i++ )
      b[i] -= L[k][i-k-1]*b[k];
  }
  l = 1;
  for( i = n-1; i >= 0; i-- ) /* Forward substitution */
  {
    dum = b[i];
    for( k = 1; k < l; k++ )
      dum -= A[i][k]*b[k+i];
    b[i] = dum/A[i][0];
    if( l < mm ) l++;
  }
}
// template instantiation
template GULAPI void BandBackSubstitution( 
           int n, int m1, int m2, const Ptr< Ptr<float> >& A, 
           const Ptr< Ptr<float> >& L, const Ptr<int>& index, Ptr<float>& b );
template GULAPI void BandBackSubstitution( 
        int n, int m1, int m2, const Ptr< Ptr<double> >& A, 
        const Ptr< Ptr<double> >& L, const Ptr<int>& index, Ptr<double>& b );


/*------------------------------------------------------------------------*//**
  Singular Value Decomposition.
  =============================

  A = U * W * V^{T} with

    A = m x n Matrix     (m >= n)
    U = m x n Matrix
    W = n x n Matrix
    V = n x n Matrix
        
    U,V orthogonal ( i.e. U^{T} * U = (1), V^{T} * V = (1) )
    W diagonal matrix, containing the singular values.                       */
/*---------------------------------------------------------------------------*/
template< class T >
GULAPI void SVDecomposition( int m, int n, Ptr< Ptr<T> >& a,
                          Ptr<T>& w, Ptr< Ptr<T> >& v )
{
  int flag,i,its,j,jj,k,l,nm;
  T anorm,c,f,g,h,s,scale,x,y,z;
  Ptr<T> rv1;

  rv1.reserve_pool(n+1);

/* ----- Householder reduction to bidiagonal form ------------------- */

  g = scale = anorm = 0.0;
  
  for( i = 1; i <= n; i++ )
  {
    l = i+1;
    rv1[i-1] = scale * g;
    g = s = scale = 0.0;
    
    if( i <= m )
    {
      for( k = i; k <=m; k++ )
        scale += rtr<T>::fabs( a[k-1][i-1] );
      if( scale != 0.0 )
      {
        for( k = i; k <= m; k++ )
        {
          a[k-1][i-1] /= scale;
          s += a[k-1][i-1] * a[k-1][i-1];
        }
        f = a[i-1][i-1];
        g = -Sign( rtr<T>::sqrt(s), f );
        h = f*g - s;
        a[i-1][i-1] = f-g;
        
        for( j=l; j<=n; j++ )
        {
          for( s=0.0,k=i; k<=m; k++ )
            s += a[k-1][i-1] * a[k-1][j-1];
          f = s / h;
          for( k = i; k <= m; k++ )
            a[k-1][j-1] += f * a[k-1][i-1];  
        }    
        for( k = i; k <=m; k++ )
          a[k-1][i-1] *= scale;
      }
    }
    w[i-1] = scale * g;
    g = s = scale = 0.0;
    
    if( (i <= m) && (i != n) )
    {
      for( k = l; k <= n; k++ )
        scale += rtr<T>::fabs( a[i-1][k-1] );
        
      if( scale != 0.0 )
      {  
        for( k = l; k <= n; k++ )
        {
          a[i-1][k-1] /= scale;
          s += a[i-1][k-1] * a[i-1][k-1];
        }

        f = a[i-1][l-1];
        g = -Sign( rtr<T>::sqrt(s), f );
        h = f*g - s;
        a[i-1][l-1] = f-g;
        
        for( k = l; k <= n; k++ )
          rv1[k-1] = a[i-1][k-1] / h;
          
        for( j = l; j <= m; j++ )
        {
          for( s = 0.0,k=l; k <= n; k++ )
            s += a[j-1][k-1] * a[i-1][k-1];
          for( k = l; k <= n; k++ )
            a[j-1][k-1] += s * rv1[k-1];
        }      
        
        for( k = l; k <= n; k++ )
          a[i-1][k-1] *= scale;          
      }  
    }
    anorm = Max( anorm, (rtr<T>::fabs(w[i-1]) + rtr<T>::fabs(rv1[i-1])) );
  }  
 /* ------- akkumulation of right side transformations ------------- */

  for( i = n; i >= 1; i-- ) 
  {
    if( i < n )
    {
      if( g != 0.0 )
      {
        for( j = l; j <= n; j++ )
          v[j-1][i-1] = (a[i-1][j-1] / a[i-1][l-1]) / g;
        for( j = l; j <= n; j++ )
        {
          for( s=0.0,k=l; k<=n; k++ )
            s += a[i-1][k-1] * v[k-1][j-1];
          for( k = l; k <= n; k++ )
            v[k-1][j-1] += s * v[k-1][i-1];
        }          
      }
      for( j = l; j <= n; j++ )
        v[i-1][j-1] = v[j-1][i-1] = 0.0;
    }
    v[i-1][i-1] = 1.0;
    g = rv1[i-1];
    l = i;
  }    
/* -------- accumulation of left side transformations --------------- */

  for( i = Min(m,n); i >=1; i-- ) 
  {
    l = i+1;
    g = w[i-1];
    
    for( j = l; j <= n; j++ )
      a[i-1][j-1] = 0.0;
      
    if( g != 0.0 )
    {
      g = (T)1 / g;
      
      for( j = l; j <= n; j++ )
      {
        for( s=0.0,k=l; k <= m; k++ )
          s += a[k-1][i-1] * a[k-1][j-1];
        
        f = (s/a[i-1][i-1]) * g;

        for( k = i; k <= m; k++ )
          a[k-1][j-1] += f * a[k-1][i-1];
      }

      for( j = i; j <= m; j++ )
        a[j-1][i-1] *= g;      
    }  
    else
      for( j = i; j <= m; j++ )
        a[j-1][i-1] = 0.0;
        
    a[i-1][i-1] += 1.0;    
  }        
/* -------- Diagonalize the bidiagonal form --------------------- */
    
  for( k = n; k >= 1; k-- )      
  {
    for( its = 1; its <= 30; its++ )
    {
      flag = 1;
      
      for( l = k; l >= 1; l-- )
      {
        nm = l-1;
        if( rtr<T>::fabs(rv1[l-1]) + anorm == anorm )
        {
          flag = 0;
          break;
        }  
        if( rtr<T>::fabs(w[nm-1]) + anorm == anorm )
          break;
      } 
      if( flag != 0 )
      {
        c = 0.0;
        s = 1.0;
        for( i = l; i <= k; i++ )
        {
          f = s * rv1[i-1];
          rv1[i-1] = c * rv1[i-1];
          
          if( rtr<T>::fabs(f) + anorm == anorm )
            break;
            
          g = w[i-1];
          h = Pythagoras( f, g );
          w[i-1] = h;
          h = (T)1 / h;
          c = g * h;
          s = -f * h;
          
          for( j = 1; j <= m; j++ )
          {
            y = a[j-1][nm-1];
            z = a[j-1][i-1];
            a[j-1][nm-1] = y * c + z * s;
            a[j-1][i-1] = z * c - y * s;
          }
        }
      }
      z = w[k-1];
      if( l == k )
      {
        if( z < 0.0 )
        {
          w[k-1] = -z;
          for( j = 1; j <= n; j++ )
            v[j-1][k-1] = -v[j-1][k-1];
        }
        break;
      }              
      if( its == 30 )
        throw InternalError();
      
      x = w[l-1];
      nm = k-1;
      y = w[nm-1];
      g = rv1[nm-1];
      h = rv1[k-1];
      f = ((y-z)*(y+z)+(g-h)*(g+h))/((T)2*h*y);
      g = Pythagoras( f, (T)1 );
      f = ((x-z)*(x+z) + h*((y/(f+Sign(g,f)))-h)) / x;
      c = s = 1.0;
      
      for( j = l; j <= nm; j++ )
      {
        i = j+1;
        g = rv1[i-1];
        y = w[i-1];
        h = s * g;
        g = c * g;
        z = Pythagoras( f, h );
        rv1[j-1] = z;
        c = f/z;
        s = h/z;
        f = x*c + g*s;
        g = g*c - x*s;
        h = y*s;
        y *= c;
        
        for( jj = 1; jj <= n; jj++ )
        {
          x = v[jj-1][j-1];
          z = v[jj-1][i-1];
          v[jj-1][j-1] = x*c + z*s;
          v[jj-1][i-1] = z*c - x*s;
        }
        z = Pythagoras( f, h );
        w[j-1] = z;
        if( z != 0.0 )
        {
          z = (T)1 / z;
          c = f*z;
          s = h*z;
        }
        f = c*g + s*y;
        x = c*y - s*g;
        
        for( jj = 1; jj <= m; jj++ )
        {
          y = a[jj-1][j-1];
          z = a[jj-1][i-1];
          a[jj-1][j-1] = y*c + z*s;
          a[jj-1][i-1] = z*c - y*s;
        }
      }
      rv1[l-1] = 0.0;
      rv1[k-1] = f;
      w[k-1] = x;
    }
  }
}    
template GULAPI void SVDecomposition( int m, int n, Ptr< Ptr<float> >& a,
                                   Ptr<float>& w, Ptr< Ptr<float> >& v );
template GULAPI void SVDecomposition( int m, int n, Ptr< Ptr<double> >& a,
                                   Ptr<double>& w, Ptr< Ptr<double> >& v );

/*-------------------------------------------------------------------*//**
  Solve system of linear equations, when a Singular Value Decomposition
  is given                                                              */
/*----------------------------------------------------------------------*/  
template< class T >            
GULAPI void SVBackSubstitution( int m, int n,
                         Ptr< Ptr<T> >& u, Ptr<T>& w, 
                         Ptr< Ptr<T> >& v, Ptr<T>& b,
                         Ptr<T>& x ) 
{
  int jj,j,i;
  T s;
  Ptr<T> tmp;
  
  tmp.reserve_pool(n);
  
  for( j = 1; j <= n; j++ )
  {
    s = 0.0;
    if( w[j-1] != 0.0 )
    {
      for( i = 1; i <= m; i++ )
        s += u[i-1][j-1] * b[i-1];
       s /= w[j-1];
    }
    tmp[j-1] = s;
  }  
  for( j = 1; j <= n; j++ )
  {
    s = 0.0;
    for ( jj = 1; jj <= n; jj++ )
      s += v[j-1][jj-1] * tmp[jj-1];
    x[j-1] = s;
  }      
  return;
}          
template GULAPI void SVBackSubstitution( int m, int n,
                         Ptr< Ptr<float> >& u, Ptr<float>& w, 
                         Ptr< Ptr<float> >& v, Ptr<float>& b,
                         Ptr<float>& x );
template GULAPI void SVBackSubstitution( int m, int n,
                         Ptr< Ptr<double> >& u, Ptr<double>& w, 
                         Ptr< Ptr<double> >& v, Ptr<double>& b,
                         Ptr<double>& x );


/*-------------------------------------------------------------------*//**
  Solve system of linear equations, when a Singular Value Decomposition
  is given                                                              */
/*----------------------------------------------------------------------*/  
template< class T >            
GULAPI void SVBackSubstitution( int m, int n,
                         Ptr< Ptr<T> >& u, Ptr<T>& w, 
                         Ptr< Ptr<T> >& v, Ptr<T>& b ) 
{
  int jj,j,i;
  T s;
  Ptr<T> tmp;
  
  tmp.reserve_pool(n);
  
  for( j = 1; j <= n; j++ )
  {
    s = 0.0;
    if( w[j-1] != 0.0 )
    {
      for( i = 1; i <= m; i++ )
        s += u[i-1][j-1] * b[i-1];
       s /= w[j-1];
    }
    tmp[j-1] = s;
  }  
  for( j = 1; j <= n; j++ )
  {
    s = 0.0;
    for ( jj = 1; jj <= n; jj++ )
      s += v[j-1][jj-1] * tmp[jj-1];
    b[j-1] = s;
  }      
  return;
}          
template GULAPI void SVBackSubstitution( int m, int n,
                         Ptr< Ptr<float> >& u, Ptr<float>& w, 
                         Ptr< Ptr<float> >& v, Ptr<float>& b );
template GULAPI void SVBackSubstitution( int m, int n,
                         Ptr< Ptr<double> >& u, Ptr<double>& w, 
                         Ptr< Ptr<double> >& v, Ptr<double>& b );

/*---------------------------------------------------------------------*//**
  Sets very small singular values to zero. This avoids disastrous
  rounding errors, when the coefficient matrix of a system of
  linear equations is ill-conditioned.
  If some singular values were zero, or had to be set to zero, the function
  returns false, else it returns true                                     */
/*------------------------------------------------------------------------*/
template< class T >
GULAPI bool SVZero( int n, Ptr<T>& w )
{
  T wmax,wmin;
  int j;
  bool well_conditioned;
  T limit = (T)n * rtr<T>::epsilon();

  wmax = wmin = rtr<T>::fabs(w[0]);
  for( j = 2; j <= n; j++ )
  {
    if( rtr<T>::fabs(w[j-1]) > wmax ) wmax = rtr<T>::fabs(w[j-1]);
    else if( rtr<T>::fabs(w[j-1]) < wmin ) wmin = rtr<T>::fabs(w[j-1]);
  }
  // ill conditioned ?
  if( (wmax == 0.0) ||
      (wmin / wmax < limit) )
    well_conditioned = false;   
  else
    well_conditioned = true;

  /*
  cout << "wmin/wmax = " << wmin / wmax << "\n";
  */
                                    
  wmin = wmax * limit;
  
  for( j = 1; j <= n; j++ )
    if( wmin > rtr<T>::fabs(w[j-1]) ) w[j-1] = 0.0;

  return well_conditioned;
}    
template GULAPI bool SVZero( int n, Ptr<float>& w );
template GULAPI bool SVZero( int n, Ptr<double>& w );


/*-----------------------------------------------------------------------*//**
  Intersect two lines in 3-dimensions, using singular decomposition.
  (remark: this is a overkill, if you alredy know that the two lines 
  intersect (but on the other hand this is the first oppurtinity
  to do something senseful with the above SVD algorithm:)))))

  retcode = 0  => intersection point found
  retcode = 1  => lines are parallel
  retcode = 2  => lines are not parallel, but don't intersect either
                  (what's the english word for "windschief" ?)
  In the last two cases the solution minimizes |x|^2 or |Ax + b|,
  and the midst of the two points on the two lines is returned.              */
/*---------------------------------------------------------------------------*/
template< class T >
int SVDIntersectLines( point<T> P1, point<T> T1,
                       point<T> P2, point<T> T2,
                       T *alpha1, T *alpha2, point<T> *P )
{
  Ptr< Ptr<T> > u,v; 
  Ptr<T> w, x, b;
  point<T> L1,L2;
  int code,i;

  u.reserve_pool(3);
  for( i = 0; i < 3; i++ )
    u[i].reserve_pool(2);
  v.reserve_pool(2);
  for( i = 0; i < 2; i++ )
    v[i].reserve_pool(2);
  w.reserve_pool(2);
  x.reserve_pool(2);
  b.reserve_pool(3);

  code = 0;

  u[0][0] = T1.x;  u[0][1] = -T2.x;
  u[1][0] = T1.y;  u[1][1] = -T2.y;
  u[2][0] = T1.z;  u[2][1] = -T2.z;

  SVDecomposition( 3, 2, u, w, v );
  
  if( SVZero( 2, w ) ) // are the lines parallel ?
    code = 1;  
    
  b[0] = P2.x - P1.x;
  b[1] = P2.y - P1.y;
  b[2] = P2.z - P1.z;

  SVBackSubstitution( 3, 2, u, w, v, b, x );

  L1.x = P1.x + x[0] * T1.x; 
  L1.y = P1.y + x[0] * T1.y; 
  L1.z = P1.z + x[0] * T1.z; 

  L2.x = P2.x + x[1] * T2.x; 
  L2.y = P2.y + x[1] * T2.y; 
  L2.z = P2.z + x[1] * T2.z; 

  // not parallel, but points too far away from each other ?
  if( !code && !rel_equal(L1, L2, rtr<T>::zero_tol() ) )
    code = 2;

  P->x = (L1.x + L2.x) / (T)2; 
  P->y = (L1.y + L2.y) / (T)2; 
  P->z = (L1.z + L2.z) / (T)2; 

  *alpha1 = x[0];
  *alpha2 = x[1];

  return code;
}
template int SVDIntersectLines( point<float> P1, point<float> T1,
                      point<float> P2, point<float> T2,
                      float *alpha1, float *alpha2, point<float> *P );
template int SVDIntersectLines( point<double> P1, point<double> T1,
                    point<double> P2, point<double> T2,
                    double *alpha1, double *alpha2, point<double> *P );







#if 0
/*----------------------------------------------------------------------

A email I found in comp.graphics.algorithms:
============================================

If you have a set of points in homogenus coordinates [x y z 1]' (note that w
is normalized to 1). You should calculate the sum of outer products of all
the coordinates:

A = sum( [x y z 1]' * [x y z 1] )

That will give you a positive definite symmetric 4x4 matrix. Now calculate
the eigenvector that corresponds to the smallest eigenvalue of A, and call
it [a b c d]'. Now the planes equation is:

0 = [a b c d] * [x y z 1]' = a*x + b*y + c*z +d

This equation minimizes the sum of squared distances betwen the points and
the plane (measured perpendicular to the plane). Degenerate cases where it's
unclear what plane to choose, will show up as multiple small eigenvalues of A.

/Henrik Gustavsson

> Elie Jamaa wrote
>
> I'm trying to write a plug-in that collapses a cloud of  n points (n>3
> of course) onto their average plane .
> Does anybody know of a formula that relates the plane's equation (maybe
> cartesian) to the xyz coordinates of the n points ?
>
> I know that there can be several roots if, for example, I have 8 points
> that are at the corners of a cube, then 3 planes are valid (like the xy, yz,
> xz planes).
> Also, there can be an inifinity of solutions if the points are aligned.
> I guess the formula would be a division by zero or something funky like
> that in those cases.
> But I don't intend to find out the roots : the points will just not be
> flattened if there isn't only one average plane.
>
> Any idea ?

-------------------------------------------------------------------------*/

void FindPlane( int nP, const Ptr< point<T> >& P )
{
  A.reserve_pool(4); for( i=0; i<4; i++ ) A[i].reserve_pool(4);
  V.reserve_pool(4); for( i=0; i<4; i++ ) V[i].reserve_pool(4);
  w.reserve_pool(4);

  for( i = 0; i < 4; i++ )
    for( j = 0; j < 4; j++ )
      A[i][j] = (T)0;

  for( i = 0; i < nP; i++ )
  {
    x = P[i].x; y = P[i].y; z = P[i].y;

    A[0][0] += x*x;  A[0][1] += x*y;  A[0][2] += x*z;  A[0][3] += x;
    A[1][0] += y*x;  A[1][1] += y*y;  A[1][2] += y*z;  A[1][3] += y;
    A[2][0] += z*x;  A[2][1] += z*y;  A[2][2] += z*z;  A[2][3] += z;
    A[3][0] += x;    A[3][1] += y;    A[3][2] += z;    A[3][3] += (T)1;
  }

  /* I think I will calc the Eigenvectors with Jacobi Transformation
     (in Numerical Recipes) */
}
#endif

}

---