---

gunu_global_approximate.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.
 */

#include "stdafx.h"

#include <iostream>

#include "gul_types.h"
#include "gunu_parametrize.h"
#include "guma_lineq.h"
#include "gunu_basics.h"
#include "gul_matrix.h"
#include "gul_io.h"

namespace gunu {

using gul::Ptr;
using gul::point;
using gul::point1;
using gul::point2;
using guma::BandDecomposition;
using guma::BandBackSubstitution;
using gul::Min;
using gul::Max;
using gul::MMProduct;
using gul::dump_matrix;
using gul::dump_vector;
using guma::LUDecomposition;
using guma::LUBackSubstitution;
using guma::SVDecomposition;
using guma::SVZero;
using guma::SVBackSubstitution;
using gul::ndebug;
using gul::Assert;
using gul::InternalError;

using std::cout;

template< class T >
void CASolve( int nQ, const Ptr< point<T> >& Q, const Ptr<T>& Uq,
              int n, int p, const Ptr<T>& U, Ptr<int>& M,
              Ptr< Ptr< Ptr<T> > >& N, Ptr< Ptr<T> >& A,
              Ptr< point<T> >& P )
{
  int i,j,k,sign,offset,bw,shift;
  Ptr<T> Rx,Ry,Rz;
  Ptr< Ptr<T> > L;
  Ptr<int> index; 
  point<T> r;

  Rx.reserve_pool(n-1);
  Ry.reserve_pool(n-1);
  Rz.reserve_pool(n-1);
  for( i = 0; i < n-1; i++ )
  {
    Rx[i] = (T)0;
    Ry[i] = (T)0;
    Rz[i] = (T)0;
  }

  // process points in first knotspan

  for( i = 1; i < M[0]; i++ )    
  {
    r = N[0][0][i] * Q[0];
    if( n-p == 0 )               // Q[i] element of first and last knot span 
      r = r + N[0][p][i]*Q[nQ-1];   // (then there is only one span)

    for( j = 1; j <= Min(p,n-1); j++ )
    {
      Rx[j-1] += N[0][j][i] * (Q[i].x - r.x);
      Ry[j-1] += N[0][j][i] * (Q[i].y - r.y);
      Rz[j-1] += N[0][j][i] * (Q[i].z - r.z);

      /*
      cout << "Rx[" << j-1 << "] (first span, Q[" << i << "]): " 
           << Rx[j-1] << "\n";
      */
    }
  }

  // process points between firts and last knotspan

  offset = i;
  for( k = 1; k < n-p; k++ ) 
  {
    for( i = 0; i < M[k]; i++ )
    {
      for( j = 0; j <= p; j++ )
      {
        Rx[k+j-1] += N[k][j][i] * Q[offset+i].x;
        Ry[k+j-1] += N[k][j][i] * Q[offset+i].y;
        Rz[k+j-1] += N[k][j][i] * Q[offset+i].z;

        /*
        cout << "Rx[" << k+j-1 << "] (Q[" << offset+i << "]): " 
             << Rx[k+j-1] << "\n";
        */
      }
    }
    offset += M[k];
  }   

  // process points in last knotspan

  if( n-p > 0 )  // only when more then one knotspan
  {
    for( i = 0; i < M[k]-1; i++ )
    {
      r = N[k][p][i] * Q[nQ-1];

      for( j = 0; j < p; j++ )
      {
        Rx[k+j-1] += N[k][j][i] * (Q[offset+i].x - r.x);
        Ry[k+j-1] += N[k][j][i] * (Q[offset+i].y - r.y);
        Rz[k+j-1] += N[k][j][i] * (Q[offset+i].z - r.z);

        /*
        cout << "Rx[" << k+j-1 << "] (last span, Q[" << offset+i << "]): " 
             << Rx[k+j-1] << "\n";
        */
      }
    }
  }
  N.reset();  // not needed anymore
  M.reset();  // not needed anymore

  /*
  cout << "Right side for X (version 2):\n";
  cout << dump_vector<T>(n-1,Rx);
  cout << "Right side for Y (version 2):\n";
  cout << dump_vector<T>(n-1,Ry);
  cout << "Right side for Z (version 2):\n";
  cout << dump_vector<T>(n-1,Rz);
  */

  L.reserve_pool(n-1);
  for( i = 0; i < n-1; i++ )
    L[i].reserve_pool(p);
  index.reserve_pool(n-1);

  if( n-1 < p )  // band width adjustment necessary
  {
    bw = n-1;
    shift = p - bw;

    for( i = 0;  i < n-1; i++ )
    {
      offset = bw-i;
      for( j = 0; j < bw+bw+1-offset; j++ ) 
        A[i][offset+j] = A[i][offset+j+shift];
    }
  } 
  else
    bw = p;

  /*
  cout << "MATRIX A (1):\n";
  cout << dump_matrix<T>(n-1,p+p+1,A);
  */

  BandDecomposition( n-1, bw, bw, A, L, &sign, index );

  /*
  cout << "MATRIX A (1):\n";
  cout << dump_matrix<T>(n-1,p+p+1,A);
  cout << "MATRIX L (1):\n";
  cout << dump_matrix<T>(n-1,p,L);
  */

  BandBackSubstitution( n-1, bw, bw, A, L, index, Rx ); 
  for( i = 0; i < n-1; i++ ) P[i+1].x = Rx[i];
  BandBackSubstitution( n-1, bw, bw, A, L, index, Ry ); 
  for( i = 0; i < n-1; i++ ) P[i+1].y = Ry[i];
  BandBackSubstitution( n-1, bw, bw, A, L, index, Rz ); 
  for( i = 0; i < n-1; i++ ) P[i+1].z = Rz[i];

  P[0] = Q[0];
  P[n] = Q[nQ-1];
}

template< class T >
void CASolve( int nQ, const Ptr< point2<T> >& Q, const Ptr<T>& Uq,
              int n, int p, const Ptr<T>& U, Ptr<int>& M,
              Ptr< Ptr< Ptr<T> > >& N, Ptr< Ptr<T> >& A,
              Ptr< point2<T> >& P )
{
  int i,j,k,sign,offset,bw,shift;
  Ptr<T> Rx,Ry,Rz;
  Ptr< Ptr<T> > L;
  Ptr<int> index; 
  point2<T> r;

  Rx.reserve_pool(n-1);
  Ry.reserve_pool(n-1);
  for( i = 0; i < n-1; i++ )
  {
    Rx[i] = (T)0;
    Ry[i] = (T)0;
  }
  // process points in first knotspan

  for( i = 1; i < M[0]; i++ )    
  {
    r = N[0][0][i] * Q[0];
    if( n-p == 0 )               // Q[i] element of first and last knot span 
      r = r + N[0][p][i]*Q[nQ-1];   // (then there is only one span)

    for( j = 1; j <= Min(p,n-1); j++ )
    {
      Rx[j-1] += N[0][j][i] * (Q[i].x - r.x);
      Ry[j-1] += N[0][j][i] * (Q[i].y - r.y);
    }
  }

  // process points between firts and last knotspan

  offset = i;
  for( k = 1; k < n-p; k++ ) 
  {
    for( i = 0; i < M[k]; i++ )
    {
      for( j = 0; j <= p; j++ )
      {
        Rx[k+j-1] += N[k][j][i] * Q[offset+i].x;
        Ry[k+j-1] += N[k][j][i] * Q[offset+i].y;
      }
    }
    offset += M[k];
  }   

  // process points in last knotspan

  if( n-p > 0 )  // only when more then one knotspan
  {
    for( i = 0; i < M[k]-1; i++ )
    {
      r = N[k][p][i] * Q[nQ-1];

      for( j = 0; j < p; j++ )
      {
        Rx[k+j-1] += N[k][j][i] * (Q[offset+i].x - r.x);
        Ry[k+j-1] += N[k][j][i] * (Q[offset+i].y - r.y);
      }
    }
  }
  N.reset();  // not needed anymore
  M.reset();  // not needed anymore

  L.reserve_pool(n-1);
  for( i = 0; i < n-1; i++ )
    L[i].reserve_pool(p+p+1);
  index.reserve_pool(n-1);

  if( n-1 < p )  // band width adjustment necessary
  {
    bw = n-1;
    shift = p - bw;

    for( i = 0;  i < n-1; i++ )
    {
      offset = bw-i;
      for( j = 0; j < bw+bw+1-offset; j++ ) 
        A[i][offset+j] = A[i][offset+j+shift];
    }
  } 
  else
    bw = p;

  BandDecomposition( n-1, bw, bw, A, L, &sign, index );

  BandBackSubstitution( n-1, bw, bw, A, L, index, Rx ); 
  for( i = 0; i < n-1; i++ ) P[i+1].x = Rx[i];
  BandBackSubstitution( n-1, bw, bw, A, L, index, Ry ); 
  for( i = 0; i < n-1; i++ ) P[i+1].y = Ry[i];

  P[0] = Q[0];
  P[n] = Q[nQ-1];
}

template< class T >
void CASolveToCol( 
                     int nQ, const Ptr< point<T> >& Q, const Ptr<T>& Uq,
                     int n, int p, const Ptr<T>& U, const Ptr<int>& M,
                     const Ptr< Ptr< Ptr<T> > >& N, Ptr< Ptr<T> >& A,
                     Ptr< Ptr<T> >& L, Ptr<int> index,
                     int iCol, Ptr< Ptr< point<T> > >& P )
{
  int i,j,k,offset;
  Ptr<T> Rx,Ry,Rz;
  point<T> r;

  Rx.reserve_pool(n-1);
  Ry.reserve_pool(n-1);
  Rz.reserve_pool(n-1);
  for( i = 0; i < n-1; i++ )
  {
    Rx[i] = (T)0;
    Ry[i] = (T)0;
    Rz[i] = (T)0;
  }

  // process points in first knotspan

  for( i = 1; i < M[0]; i++ )    
  {
    r = N[0][0][i] * Q[0];
    if( n-p == 0 )               // Q[i] element of first and last knot span 
      r = r + N[0][p][i]*Q[nQ-1];   // (then there is only one span)

    for( j = 1; j <= Min(p,n-1); j++ )
    {
      Rx[j-1] += N[0][j][i] * (Q[i].x - r.x);
      Ry[j-1] += N[0][j][i] * (Q[i].y - r.y);
      Rz[j-1] += N[0][j][i] * (Q[i].z - r.z);
    }
  }

  // process points between first and last knotspan

  offset = i;
  for( k = 1; k < n-p; k++ ) 
  {
    for( i = 0; i < M[k]; i++ )
    {
      for( j = 0; j <= p; j++ )
      {
        Rx[k+j-1] += N[k][j][i] * Q[offset+i].x;
        Ry[k+j-1] += N[k][j][i] * Q[offset+i].y;
        Rz[k+j-1] += N[k][j][i] * Q[offset+i].z;
      }
    }
    offset += M[k];
  }   

  // process points in last knotspan

  if( n-p > 0 )  // only when more then one knotspan
  {
    for( i = 0; i < M[k]-1; i++ )
    {
      r = N[k][p][i] * Q[nQ-1];

      for( j = 0; j < p; j++ )
      {
        Rx[k+j-1] += N[k][j][i] * (Q[offset+i].x - r.x);
        Ry[k+j-1] += N[k][j][i] * (Q[offset+i].y - r.y);
        Rz[k+j-1] += N[k][j][i] * (Q[offset+i].z - r.z);
      }
    }
  }

  // calculate the x coordinates of the controlpoints
  BandBackSubstitution( n-1, Min(p,n-1), Min(p,n-1), A, L, index, Rx ); 
  for( i = 0; i < n-1; i++ ) P[i+1][iCol].x = Rx[i];
  BandBackSubstitution( n-1, Min(p,n-1), Min(p,n-1), A, L, index, Ry ); 
  for( i = 0; i < n-1; i++ ) P[i+1][iCol].y = Ry[i];
  BandBackSubstitution( n-1, Min(p,n-1), Min(p,n-1), A, L, index, Rz ); 
  for( i = 0; i < n-1; i++ ) P[i+1][iCol].z = Rz[i];

  P[0][iCol] = Q[0];
  P[n][iCol] = Q[nQ-1];
}

template< class T >
void CACalcNTN( int nQ, const Ptr<T>& Uq,
                int n, int p, const Ptr<T>& U, Ptr<int>& M,
                Ptr< Ptr< Ptr<T> > >& N, Ptr< Ptr<T> >& A )
{
  Ptr<T> B;
  T u,sum;
  int offset,span,i,j,k,l,m;

  B.reserve_pool(p+1);

  span = p;
  offset = 0;

  for( i = 0; i < nQ; i++ )
  {
    u = Uq[i];

    while( (u >= U[span+1]) && (span != n) )
    {
      M[span-p] = i - offset;  // number of points in previous knot span 
      span++;
      offset = i;
    }
    CalcBasisFunctions( u, span, p, U, B );

    for( j = 0; j <= p; j++ )
      N[span-p][j][i-offset] = B[j];
  }
  M[n-p] = nQ - offset;  // number of points in last knot span
  
  /* 
   * calculate A = N^T * N of the normal equation (as a band matrix),
   * but without the first and last row/column. This doesn't affects
   * the remaining matrix, since the knot vector is clamped 
   */

  for( i = 1; i < n; i++ )
  {
    m = i-p;

    for( j = Max(0,-m); j <= p; j++ ) // subdiagonals
    {
      sum = (T)0;
      for( k = Min(j,n-i); k >= Max(0,-m); k-- )
      {
        for( l = 0; l < M[m+k]; l++ )
          sum += N[m+k][p-k][l]*N[m+k][j-k][l];
      }
      A[i-1][j] = sum;
    }
    for( j = Max(0,i+p-n); j < p; j++ ) // superdiagonals
    {
      sum = (T)0;
      for( k = Max(0,i+p-n); k <= Min(j,i); k++ )
      {
        for( l = 0; l < M[i-k]; l++ )
          sum += N[i-k][k][l]*N[i-k][p-j+k][l];
      }
      A[i-1][p+p-j] = sum;
    }
  }
}

/*----------------------------------------------------------------------
  Least squares approximation of a set of data points with a curve of a
  certain degree, number of knots and number of control points
  (knot vector of the curve and parameter values of the data points have
  to be provided by the caller; also the maximum number of data points per
  knot span must be given) 
-----------------------------------------------------------------------*/
template< class T, class EP >
GULAPI bool DoGlobalCurveApproximation(
          int nQ, 
          const Ptr<EP>& Q, const 
          Ptr<T>& QU,
          int n, 
          int p, 
          const Ptr<T>& U, 
          int max_pts_span, 
          Ptr<EP>& P )
{
  Ptr< Ptr< Ptr<T> > > N;
  int i,j;
  Ptr< Ptr<T> > A;
  Ptr<int> M;
 
  if( p < 2 ) return false;
  if( n < p ) return false;
  if( nQ <= n+1 ) return false; // else no unique minimum

  M.reserve_pool(n-p+1);
  N.reserve_pool(n-p+1);
  for( i = 0; i <= n-p; i++ ) {
    N[i].reserve_pool(p+1);
    for( j = 0; j <= p; j++ ) N[i][j].reserve_pool(max_pts_span);
  }
  A.reserve_pool(n-1);
  for( i = 0; i < n-1; i++ )
    A[i].reserve_pool(p+p+1);

  CACalcNTN<T>( nQ, QU, n, p, U, M, N, A );

  // cout << "MATRIX NTN (version 2):\n";
  // cout << dump_matrix<T>(n-1,p+p+1,A);

  CASolve( nQ, Q, QU, n, p, U, M, N, A, P );
  
  return true;
}
// template instantiation 
template GULAPI bool DoGlobalCurveApproximation(
          int nQ, 
          const Ptr< point<float> >& Q, const 
          Ptr<float>& QU,
          int n, 
          int p, 
          const Ptr<float>& U, 
          int max_pts_span, 
          Ptr< point<float> >& P );
template GULAPI bool DoGlobalCurveApproximation(
          int nQ, 
          const Ptr< point<double> >& Q, const 
          Ptr<double>& QU,
          int n, 
          int p, 
          const Ptr<double>& U, 
          int max_pts_span, 
          Ptr< point<double> >& P );
template GULAPI bool DoGlobalCurveApproximation(
          int nQ, 
          const Ptr< point2<float> >& Q, const 
          Ptr<float>& QU,
          int n, 
          int p, 
          const Ptr<float>& U, 
          int max_pts_span, 
          Ptr< point2<float> >& P );
template GULAPI bool DoGlobalCurveApproximation(
          int nQ, 
          const Ptr< point2<double> >& Q, const 
          Ptr<double>& QU,
          int n, 
          int p, 
          const Ptr<double>& U, 
          int max_pts_span, 
          Ptr< point2<double> >& P );

/*----------------------------------------------------------------------
  Least squares approximation of a set of data points with a curve of a
  certain degree, number of knots and number of control points
-----------------------------------------------------------------------*/
template< class T, class EP >
GULAPI bool GlobalCurveApproximation(int nQ, const Ptr<EP>& Q,
                         int n, int p, Ptr<T>& U, Ptr<EP>& P )
{
  Ptr<T> Uq,d;
  T sum;
  Ptr< Ptr< Ptr<T> > > N;
  int Mmax;
  Ptr< Ptr<T> > A;
  Ptr<int> M;
 
  // return GlobalCurveApproximationDBG<T>(nQ,Q,n,p,U,P);

  d.reserve_pool(nQ);
  Uq.reserve_pool(nQ);

  sum = ChordLength( nQ, Q, d );
  if( sum == (T)0 ) return false;
  ChordLengthParameters( nQ, sum, d, Uq );
  if( !(Mmax = KnotVectorByAveraging(nQ, Uq, n, p, U)) ) return false;

  /*
  cout << "Parameters:\n";
  cout << dump_vector<T>(nQ,Uq);
  cout << "Knot Vector:\n";
  cout << dump_vector<T>(n+p+2,U);
  */

  return DoGlobalCurveApproximation(nQ,Q,Uq,n,p,U,Mmax,P);
}
// template instantiation
template GULAPI bool GlobalCurveApproximation(
               int nQ, const Ptr< point<float> >& Q,
               int n, int p, Ptr<float>& U, Ptr< point<float> >& P );
template GULAPI bool GlobalCurveApproximation(
               int nQ, const Ptr< point<double> >& Q,
               int n, int p, Ptr<double>& U, Ptr< point<double> >& P );
template GULAPI bool GlobalCurveApproximation(
               int nQ, const Ptr< point2<float> >& Q,
               int n, int p, Ptr<float>& U, Ptr< point2<float> >& P );
template GULAPI bool GlobalCurveApproximation(
               int nQ, const Ptr< point2<double> >& Q,
               int n, int p, Ptr<double>& U, Ptr< point2<double> >& P );


/*----------------------------------------------------------------------
  Least squares approximation of a set of data points with a surface
-----------------------------------------------------------------------*/
template< class T, class EP >
GULAPI bool GlobalSurfaceApproximation(
           int nRows, int nCols, const Ptr< Ptr<EP> >& Q,
           int nu, int pu, Ptr<T>& U, int nv, int pv, Ptr<T>& V,
           Ptr< Ptr<EP> > &P )
{
  Ptr<T> Uq,Vq;
  Ptr< Ptr< Ptr<T> > > N;
  int i,j,Mmaxu,Mmaxv,sign,offset,bw,shift;
  Ptr< Ptr<T> > A,L;
  Ptr< Ptr<EP> > R;
  Ptr<int> index,M;

  if( (pu < 2) || (pv < 2) ) return false;
  if( (nu < pu) || (nv < pv) ) return false;
  if( (nRows <= nv+1) || (nCols <= nu+1) ) return false;
  
  Uq.reserve_pool(nCols);
  Vq.reserve_pool(nRows);

  if( !ChordLengthMeshParams( nRows, nCols, Q, Uq, Vq ) ) return false; 

  if( !(Mmaxu = KnotVectorByAveraging(nCols, Uq, nu, pu, U)) ) return false;
  if( !(Mmaxv = KnotVectorByAveraging(nRows, Vq, nv, pv, V)) ) return false;

  // calcuate the row curves

  M.reserve_pool(nu-pu+1);
  N.reserve_pool(nu-pu+1);
  for( i = 0; i <= nu-pu; i++ ) {
    N[i].reserve_pool(pu+1);
    for( j = 0; j <= pu; j++ ) N[i][j].reserve_pool(Mmaxu);
  }
  A.reserve_pool(nu-1);
  for( i = 0; i < nu-1; i++ )
    A[i].reserve_pool(pu+pu+1);

  R.reserve_pool(nu+1);
  for( i = 0; i <= nu; i++ )
    R[i].reserve_pool(nRows);

  CACalcNTN<T>( nCols, Uq, nu, pu, U, M, N, A );

  L.reserve_pool(nu-1);
  for( i = 0; i < nu-1; i++ )
    L[i].reserve_pool(pu+pu+1);
  index.reserve_pool(nu-1);

  if( nu-1 < pu )  // band width adjustment necessary
  {
    bw = nu-1;
    shift = pu - bw;

    for( i = 0;  i < nu-1; i++ )
    {
      offset = bw-i;
      for( j = 0; j < bw+bw+1-offset; j++ ) 
        A[i][offset+j] = A[i][offset+j+shift];
    }
  } 
  else
    bw = pu;

  BandDecomposition( nu-1, bw, bw, A, L, &sign, index );

  for( i = 0; i < nRows; i++ )
  {
    CASolveToCol( nCols, Q[i], Uq, nu, pu, U, M, N, A, L, index, i, R );
  }

  // calcuate the column curves

  M.reserve_pool(nv-pv+1);
  N.reserve_pool(nv-pv+1);  // this automatically frees old content of N
  for( i = 0; i <= nv-pv; i++ ) {
    N[i].reserve_pool(pv+1);
    for( j = 0; j <= pv; j++ ) N[i][j].reserve_pool(Mmaxv);
  }
  A.reserve_pool(nv-1);
  for( i = 0; i < nv-1; i++ )
    A[i].reserve_pool(pv+pv+1);

  CACalcNTN<T>( nRows, Vq, nv, pv, V, M, N, A );

  L.reserve_pool(nv-1);
  for( i = 0; i < nv-1; i++ )
    L[i].reserve_pool(pv+pv+1);
  index.reserve_pool(nv-1);

  if( nv-1 < pv )  // band width adjustment necessary
  {
    bw = nv-1;
    shift = pv - bw;

    for( i = 0;  i < nv-1; i++ )
    {
      offset = bw-i;
      for( j = 0; j < bw+bw+1-offset; j++ ) 
        A[i][offset+j] = A[i][offset+j+shift];
    }
  } 
  else
    bw = pv;

  BandDecomposition( nv-1, bw, bw, A, L, &sign, index );

  for( i = 0; i <= nu; i++ )
  {
    CASolveToCol( nRows, R[i], Vq, nv, pv, V, M, N, A, L, index, i, P );
  }

  return true;
}
// template instantiation
template GULAPI bool GlobalSurfaceApproximation< float,point<float> >( 
           int nRows, int nCols, const Ptr< Ptr< point<float> > >& Q,
           int pu, int nu, Ptr<float>& U, int nv, int pv, Ptr<float>& V,
           Ptr< Ptr< point<float> > >& P );
template GULAPI bool GlobalSurfaceApproximation< double,point<double> >( 
           int nRows, int nCols, const Ptr< Ptr< point<double> > >& Q,
           int pu, int nu, Ptr<double>& U, int nv, int pv, Ptr<double>& V,
           Ptr< Ptr< point<double> > >& P );
/*
template bool GlobalSurfaceApproximation< float,point1<float> >( 
           int nRows, int nCols, const Ptr< Ptr< point1<float> > >& Q,
           int pu, int nu, Ptr<float>& U, int nv, int pv, Ptr<float>& V,
           Ptr< Ptr< point1<float> > >& P );
template bool GlobalSurfaceApproximation< double,point1<double> >( 
           int nRows, int nCols, const Ptr< Ptr< point1<double> > >& Q,
           int pu, int nu, Ptr<double>& U, int nv, int pv, Ptr<double>& V,
           Ptr< Ptr< point1<double> > >& P );
*/



#if 0
/*
 * Debug functions for finding errors in the band matrix solver
 *
 * I thought there was an error in the approximation function, but
 * this function delivers the same result. When the number of knotspans
 * is almost as high as the number of data points, the function does
 * a big loop between the first and second data point (with a testcase of 
 * collinear points). Though the approximation at the data points is
 * still very good, this doesn't look allright !!!!! 
 */

template< class T >
bool GlobalCurveApproximationDBG(int nQ, const Ptr< point<T> >& Q,
                         int n, int p, Ptr<T>& U, Ptr< point<T> >& P )
{
  Ptr<T> Uq,d,B,rx,ry,rz,Rx,Ry,Rz;
  T sum,u;
  int i,j,Mmax,span,sign;
  Ptr< Ptr<T> > N,NT,NTN,A;
  Ptr<int> index;

  d.reserve_pool(nQ);
  Uq.reserve_pool(nQ);

  sum = ChordLength( nQ, Q, d );
  if( sum == (T)0 ) return false;
  ChordLengthParameters( nQ, sum, d, Uq );
  if( !(Mmax = KnotVectorByAveraging(nQ, Uq, n, p, U)) ) return false;

  /*
  cout << "Parameters:\n";
  cout << dump_vector<T>(nQ,Uq);
  cout << "Knot Vector:\n";
  cout << dump_vector<T>(n+p+2,U);
  */
  
  N.reserve_pool(nQ);
  for( i = 0; i < nQ; i++ )
    N[i].reserve_pool(n+1);

  span = p;
  for( i = 0; i < nQ; i++ )
  {
    u = Uq[i];

    if( (u >= U[span+1]) && (span != n) ) span++;

    B.use_pointer(&N[i][span-p],p+1);
    CalcBasisFunctions( u, span, p, U, B );

    for( j = 0; j < span-p; j++ ) N[i][j] = (T)0;
    for( j = span+1; j <= n; j++ ) N[i][j] = (T)0;
  }

  /*
  cout << "MATRIX N:\n";
  cout << dump_matrix<T>(nQ,n+1,N);
  */

  NT.reserve_pool(n+1);
  for( i = 0; i <= n; i++ )
    NT[i].reserve_pool(nQ);

  for( i = 0; i <= n; i++ )
    for( j = 0; j < nQ; j++ )
      NT[i][j] = N[j][i];

  NTN.reserve_pool(n+1);
  for( i = 0; i <= n; i++ )
    NTN[i].reserve_pool(n+1);

  MMProduct( n+1, nQ, n+1, NT, N, NTN ); 

  /*
  cout << "MATRIX NTN:\n";
  cout << dump_matrix<T>(n+1,n+1,NTN);
  */

  rx.reserve_pool(nQ);
  ry.reserve_pool(nQ);
  rz.reserve_pool(nQ);

  for( i = 1; i < nQ-1; i++ )
  { 
    rx[i] = Q[i].x - N[i][0]*Q[0].x - N[i][n]*Q[nQ-1].x;
    ry[i] = Q[i].y - N[i][0]*Q[0].y - N[i][n]*Q[nQ-1].y;
    rz[i] = Q[i].z - N[i][0]*Q[0].z - N[i][n]*Q[nQ-1].z;
  }

  Rx.reserve_pool(n-1);
  Ry.reserve_pool(n-1);
  Rz.reserve_pool(n-1);

  for( i = 1; i < n; i++ )
  {
    Rx[i-1] = (T)0;
    Ry[i-1] = (T)0;
    Rz[i-1] = (T)0;
    for( j = 1; j < nQ-1; j++ )
    {
      Rx[i-1] += N[j][i]*rx[j];
      Ry[i-1] += N[j][i]*ry[j];
      Rz[i-1] += N[j][i]*rz[j];

      /*
      cout << "Rx[" << i-1 << "] (Q[" << j << "]): " 
           << Rx[i-1] << "\n";
      */
    }
  }

  /*
  cout << "Right side for X:\n";
  cout << dump_vector<T>(n-1,Rx);
  cout << "Right side for Y:\n";
  cout << dump_vector<T>(n-1,Ry);
  cout << "Right side for Z:\n";
  cout << dump_vector<T>(n-1,Rz);
  */

  A.reserve_pool(n-1);
  for( i = 0; i < n-1; i++ ) A[i].reserve_pool(n-1);
  index.reserve_pool(n-1);

  for( i = 0; i < n-1; i++ )
    for( j = 0; j < n-1; j++ )
      A[i][j] = NTN[i+1][j+1];

  // solve with SVD
  cout << "solving with SVD\n";

  Ptr< Ptr<T> > SV;
  Ptr<T>        Sw;

  SV.reserve_pool(n-1);
  for( i = 0; i < n-1; i++ ) SV[i].reserve_pool(n-1);
  Sw.reserve_pool(n-1);

  SVDecomposition( n-1, n-1, A, Sw, SV );

  bool illcond = !SVZero( n-1, Sw );
  if( illcond ) cout << "Coefficients matrix ill conditioned!\n";
 
  SVBackSubstitution( n-1, n-1, A, Sw, SV, Rx );
  SVBackSubstitution( n-1, n-1, A, Sw, SV, Ry );
  SVBackSubstitution( n-1, n-1, A, Sw, SV, Rz );

  /*
  // solve with LUD

  LUDecomposition( n-1, A, &sign, index );

  LUBackSubstitution( n-1, index, A, Rx );
  LUBackSubstitution( n-1, index, A, Ry );
  LUBackSubstitution( n-1, index, A, Rz );
  */

  for( i = 1; i <= n-1; i++ )
  {
    P[i].x = Rx[i-1];
    P[i].y = Ry[i-1];
    P[i].z = Rz[i-1];
  }
  P[0] = Q[0];
  P[n] = Q[nQ-1];

  return true;
}
// template instantiation
template bool GlobalCurveApproximationDBG(
               int nQ, const Ptr< point<float> >& Q,
               int n, int p, Ptr<float>& U, Ptr< point<float> >& P );
template bool GlobalCurveApproximationDBG(
               int nQ, const Ptr< point<double> >& Q,
               int n, int p, Ptr<double>& U, Ptr< point<double> >& P );


template< class T >
bool GlobalCurveApproximationDBG(int nQ, const Ptr< point2<T> >& Q,
                         int n, int p, Ptr<T>& U, Ptr< point2<T> >& P )
{
  Ptr<T> Uq,d,B,rx,ry,rz,Rx,Ry,Rz;
  T sum,u;
  int i,j,Mmax,span,sign;
  Ptr< Ptr<T> > N,NT,NTN,A;
  Ptr<int> index;

  d.reserve_pool(nQ);
  Uq.reserve_pool(nQ);

  sum = ChordLength( nQ, Q, d );
  if( sum == (T)0 ) return false;
  ChordLengthParameters( nQ, sum, d, Uq );
  if( !(Mmax = KnotVectorByAveraging(nQ, Uq, n, p, U)) ) return false;

  N.reserve_pool(nQ);
  for( i = 0; i < nQ; i++ )
    N[i].reserve_pool(n+1);

  span = p;
  for( i = 0; i < nQ; i++ )
  {
    u = Uq[i];

    if( (u >= U[span+1]) && (span != n) ) span++;

    B.use_pointer(&N[i][span-p],p+1);
    CalcBasisFunctions( u, span, p, U, B );

    for( j = 0; j < span-p; j++ ) N[i][j] = (T)0;
    for( j = span+1; j <= n; j++ ) N[i][j] = (T)0;
  }

  /*
  cout << "MATRIX N:\n";
  cout << dump_matrix<T>(nQ,n+1,N);
  */

  NT.reserve_pool(n+1);
  for( i = 0; i <= n; i++ )
    NT[i].reserve_pool(nQ);

  for( i = 0; i <= n; i++ )
    for( j = 0; j < nQ; j++ )
      NT[i][j] = N[j][i];

  NTN.reserve_pool(n+1);
  for( i = 0; i <= n; i++ )
    NTN[i].reserve_pool(n+1);

  MMProduct( n+1, nQ, n+1, NT, N, NTN ); 

  /*
  cout << "MATRIX NTN:\n";
  cout << dump_matrix<T>(n+1,n+1,NTN);
  */

  rx.reserve_pool(nQ);
  ry.reserve_pool(nQ);
  rz.reserve_pool(nQ);

  for( i = 1; i < nQ-1; i++ )
  { 
    rx[i] = Q[i].x - N[i][0]*Q[0].x - N[i][n]*Q[nQ-1].x;
    ry[i] = Q[i].y - N[i][0]*Q[0].y - N[i][n]*Q[nQ-1].y;
  }

  Rx.reserve_pool(n-1);
  Ry.reserve_pool(n-1);

  for( i = 1; i < n; i++ )
  {
    Rx[i-1] = (T)0;
    Ry[i-1] = (T)0;
    for( j = 1; j < nQ-1; j++ )
    {
      Rx[i-1] += N[j][i]*rx[j];
      Ry[i-1] += N[j][i]*ry[j];
    }
  }
  /*
  cout << "Right side for X:\n";
  cout << dump_vector<T>(n-1,Rx);
  cout << "Right side for Y:\n";
  cout << dump_vector<T>(n-1,Ry);
  cout << "Right side for Z:\n";
  cout << dump_vector<T>(n-1,Rz);
  */

  A.reserve_pool(n-1);
  for( i = 0; i < n-1; i++ ) A[i].reserve_pool(n-1);
  index.reserve_pool(n-1);

  for( i = 0; i < n-1; i++ )
    for( j = 0; j < n-1; j++ )
      A[i][j] = NTN[i+1][j+1];

  // solve with SVD
  // cout << "solving with SVD\n";

  Ptr< Ptr<T> > SV;
  Ptr<T>        Sw;

  SV.reserve_pool(n-1);
  for( i = 0; i < n-1; i++ ) SV[i].reserve_pool(n-1);
  Sw.reserve_pool(n-1);

  SVDecomposition( n-1, n-1, A, Sw, SV );

  bool illcond = !SVZero( n-1, Sw );
  if( illcond ) cout << "Coefficients matrix ill conditioned!\n";
 
  SVBackSubstitution( n-1, n-1, A, Sw, SV, Rx );
  SVBackSubstitution( n-1, n-1, A, Sw, SV, Ry );

  /*
  // solve with LUD

  LUDecomposition( n-1, A, &sign, index );

  LUBackSubstitution( n-1, index, A, Rx );
  LUBackSubstitution( n-1, index, A, Ry );
  LUBackSubstitution( n-1, index, A, Rz );
  */

  for( i = 1; i <= n-1; i++ )
  {
    P[i].x = Rx[i-1];
    P[i].y = Ry[i-1];
  }
  P[0] = Q[0];
  P[n] = Q[nQ-1];

  return true;
}
template bool GlobalCurveApproximationDBG(
               int nQ, const Ptr< point2<float> >& Q,
               int n, int p, Ptr<float>& U, Ptr< point2<float> >& P );
template bool GlobalCurveApproximationDBG(
               int nQ, const Ptr< point2<double> >& Q,
               int n, int p, Ptr<double>& U, Ptr< point2<double> >& P );
#endif

}

---