vpBSpline.cpp

/****************************************************************************
 *
 * $Id: vpBSpline.cpp 4303 2013-07-04 14:14:00Z fspindle $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2013 by INRIA. All rights reserved.
 * 
 * This software is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * ("GPL") version 2 as published by the Free Software Foundation.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact INRIA about acquiring a ViSP Professional 
 * Edition License.
 *
 * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
 * 
 * This software was developed at:
 * INRIA Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 * http://www.irisa.fr/lagadic
 *
 * If you have questions regarding the use of this file, please contact
 * INRIA at visp@inria.fr
 * 
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 *
 * Description:
 * This class implements the B-Spline
 *
 * Authors:
 * Nicolas Melchior
 *
 *****************************************************************************/

#include <visp/vpBSpline.h>
#include <visp/vpDebug.h>

vpBSpline::vpBSpline()
{
  p = 3;  //By default : clubic spline
}

vpBSpline::vpBSpline(const vpBSpline &bspline)
{
  controlPoints = bspline.controlPoints;
  knots = bspline.knots;
  p = bspline.p;
  crossingPoints = bspline.crossingPoints;
}
vpBSpline::~vpBSpline()
{
}

unsigned int 
vpBSpline::findSpan(double l_u, unsigned int l_p, std::vector<double> &l_knots)
{
  unsigned int m = (unsigned int)l_knots.size()-1;

  if(l_u > l_knots.back()) 
  {
    //vpTRACE("l_u higher than the maximum value in the knot vector  : %lf",l_u);
    return((unsigned int)(m-l_p-1));
  }

  //if (l_u == l_knots.back()) 
  if (std::fabs(l_u - l_knots.back()) <= std::fabs(vpMath::maximum(l_u, l_knots.back())) * std::numeric_limits<double>::epsilon()) 
    return((unsigned int)(m-l_p-1));

  double low = l_p;
  double high = m-l_p;
  double middle = (low+high)/2.0;

  while (l_u < l_knots[(unsigned int)vpMath::round(middle)] || l_u >= l_knots[(unsigned int)vpMath::round(middle+1)])
  {
    if(l_u < l_knots[(unsigned int)vpMath::round(middle)]) high = middle;
    else low = middle;
    middle = (low+high)/2.0;
  }

  return (unsigned int)middle;
}


unsigned int 
vpBSpline::findSpan(double u)
{
  return findSpan( u, p, knots);
}


vpBasisFunction* vpBSpline::computeBasisFuns(double l_u, unsigned int l_i, unsigned int l_p, std::vector<double> &l_knots)
{
  vpBasisFunction* N = new vpBasisFunction[l_p+1];

  N[0].value = 1.0;

  double *left = new double[l_p+1];
  double *right = new double[l_p+1];
  double saved = 0.0;
  double temp = 0.0;

  for(unsigned int j = 1; j <= l_p; j++)
  {
    left[j] = l_u - l_knots[l_i+1-j];
    right[j] = l_knots[l_i+j] - l_u;
    saved = 0.0;

    for (unsigned int r = 0; r < j; r++)
    {
      temp = N[r].value / (right[r+1]+left[j-r]);
      N[r].value = saved +right[r+1]*temp;
      saved = left[j-r]*temp;
    }
    N[j].value = saved;
  }
  for(unsigned int j = 0; j < l_p+1; j++)
  {
    N[j].i = l_i-l_p+j;
    N[j].p = l_p;
    N[j].u = l_u;
    N[j].k = 0;
  }

  delete[] left;
  delete[] right;

  return N;
}


vpBasisFunction* vpBSpline::computeBasisFuns(double u)
{
  unsigned int i = findSpan(u);
  return computeBasisFuns(u, i, p ,knots);
}


vpBasisFunction** vpBSpline::computeDersBasisFuns(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, std::vector<double> &l_knots)
{
  vpBasisFunction** N;
  N = new vpBasisFunction*[l_der+1];
  for(unsigned int j = 0; j <= l_der; j++)
    N[j] = new vpBasisFunction[l_p+1];

  vpMatrix a(2,l_p+1);
  vpMatrix ndu(l_p+1,l_p+1);
  ndu[0][0] = 1.0;

  double *left = new double[l_p+1];
  double *right = new double[l_p+1];
  double saved = 0.0;
  double temp = 0.0;

  for(unsigned int j = 1; j <= l_p; j++)
  {
    left[j] = l_u - l_knots[l_i+1-j];
    right[j] = l_knots[l_i+j] - l_u;
    saved = 0.0;

    for (unsigned int r = 0; r < j; r++)
    {
      ndu[j][r] = right[r+1]+left[j-r];
      temp = ndu[r][j-1]/ndu[j][r];
      ndu[r][j] = saved + right[r+1]*temp;
      saved = left[j-r]*temp;
    }
    ndu[j][j] = saved;
  }

  for(unsigned int j = 0; j <= l_p; j++)
  {
    N[0][j].value = ndu[j][l_p];
    N[0][j].i = l_i-l_p+j;
    N[0][j].p = l_p;
    N[0][j].u = l_u;
    N[0][j].k = 0;
  }
  
  if( l_der > l_p)
  {
    vpTRACE("l_der must be under or equal to l_p");
    l_der = l_p;
  }

  unsigned int s1,s2;
  double d;
  int rk;
  unsigned int pk;
  unsigned int j1,j2;

  for (unsigned int r = 0; r <= l_p; r++)
  {
    s1 = 0;
    s2 = 1;
    a[0][0] = 1.0;
    for(unsigned int k = 1; k <= l_der; k++)
    {
      d = 0.0;
      rk = (int)(r-k);
      pk = l_p-k;
      if(r >= k)
      {
        a[s2][0] = a[s1][0]/ndu[pk+1][rk];
        d = a[s2][0]*ndu[(unsigned int)rk][pk];
      }

      if(rk >= -1)
        j1 = 1;
      else
        j1 = (unsigned int)(-rk);

      if(r-1 <= pk)
        j2 = k-1;
      else
        j2 = l_p-r;

      for(unsigned int j =j1; j<= j2; j++)
      {
        a[s2][j] = (a[s1][j]-a[s1][j-1])/ndu[pk+1][(unsigned int)rk+j];
        d += a[s2][j]*ndu[(unsigned int)rk+j][pk];
      }

      if(r <= pk)
      {
        a[s2][k] = -a[s1][k-1]/ndu[pk+1][r];
        d += a[s2][k]*ndu[r][pk];
      }
      N[k][r].value = d;
      N[k][r].i = l_i-l_p+r;
      N[k][r].p = l_p;
      N[k][r].u = l_u;
      N[k][r].k = k;

      s1 = (s1+1)%2;
      s2 = (s2+1)%2;
    }
  }

  double r = l_p;
  for ( unsigned int k = 1; k <= l_der; k++ )
  {
    for (unsigned int j = 0; j <= l_p; j++)
       N[k][j].value *= r;
    r *= (l_p-k);
  }

  delete[] left;
  delete[] right;

  return N;
}


vpBasisFunction** vpBSpline::computeDersBasisFuns(double u, unsigned int der)
{
  unsigned int i = findSpan(u);
  return computeDersBasisFuns(u, i, p , der, knots);
}


vpImagePoint vpBSpline::computeCurvePoint(double l_u, unsigned int l_i, unsigned int l_p, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints)
{
  vpBasisFunction* N = computeBasisFuns(l_u, l_i, l_p, l_knots);
  vpImagePoint pt;

  double ic = 0;
  double jc = 0;
  for(unsigned int j = 0; j <= l_p; j++)
  {
    ic = ic + N[j].value * (l_controlPoints[l_i-l_p+j]).get_i();
    jc = jc + N[j].value * (l_controlPoints[l_i-l_p+j]).get_j();
  }

  pt.set_i(ic);
  pt.set_j(jc);

  delete[] N;

  return pt;
}


vpImagePoint vpBSpline::computeCurvePoint(double u)
{
  vpBasisFunction* N = computeBasisFuns(u);
  vpImagePoint pt;

  double ic = 0;
  double jc = 0;
  for(unsigned int j = 0; j <= p; j++)
  {
    ic = ic + N[j].value * (controlPoints[N[0].i+j]).get_i();
    jc = jc + N[j].value * (controlPoints[N[0].i+j]).get_j();
  }

  pt.set_i(ic);
  pt.set_j(jc);

  delete[] N;

  return pt;
}


vpImagePoint* vpBSpline::computeCurveDers(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints)
{
  vpImagePoint *derivate = new vpImagePoint[l_der+1];
  vpBasisFunction** N;
  N = computeDersBasisFuns(l_u, l_i, l_p, l_der, l_knots);

  unsigned int du;
  if (l_p < l_der)
  {
    vpTRACE("l_der must be under or equal to l_p");
    du = l_p;
  }
  else du = l_der;

  for(unsigned int k = 0; k <= du; k++)
  {
    derivate[k].set_ij(0.0,0.0);
    for(unsigned int j = 0; j<= l_p; j++)
    {
      derivate[k].set_i( derivate[k].get_i() + N[k][j].value*(l_controlPoints[l_i-l_p+j]).get_i());
      derivate[k].set_j( derivate[k].get_j() + N[k][j].value*(l_controlPoints[l_i-l_p+j]).get_j());
    }
  }


  for(unsigned int j = 0; j <= l_der; j++)
    delete[] N[j];
  delete[] N;


  return derivate;
}


vpImagePoint* vpBSpline::computeCurveDers(double u, unsigned int der)
{
  vpImagePoint *derivate = new vpImagePoint[der+1];
  vpBasisFunction** N;
  N = computeDersBasisFuns(u, der);

  unsigned int du;
  if (p < der)
  {
    vpTRACE("der must be under or equal to p");
    du = p;
  }
  else du = der;

  for(unsigned int k = 0; k <= du; k++)
  {
    derivate[k].set_ij(0.0,0.0);
    for(unsigned int j = 0; j<= p; j++)
    {
      derivate[k].set_i( derivate[k].get_i() + N[k][j].value*(controlPoints[N[0][0].i-p+j]).get_i());
      derivate[k].set_j( derivate[k].get_j() + N[k][j].value*(controlPoints[N[0][0].i-p+j]).get_j());
    }
  }

  for(unsigned int j = 0; j <= der; j++)
    delete[] N[j];
  delete[] N;

  return derivate;
}