Interpolator.cpp

/*
This file is part of the BIAS library (Basic ImageAlgorithmS).

Copyright (C) 2003-2009    (see file CONTACT for details)
  Multimediale Systeme der Informationsverarbeitung
  Institut fuer Informatik
  Christian-Albrechts-Universitaet Kiel


BIAS is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.

BIAS 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 Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with BIAS; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#ifdef WIN32
#  define _USE_MATH_DEFINES
#endif //WIN32

#include <math.h>
#include <iostream>
#include "Interpolator.hh"
#include "Base/Debug/Error.hh"
//#include "Base/ImageUtils/ImageDraw.hh"
#include "Lapack.hh"
#include "SVD.hh"

using namespace BIAS;
using namespace std;

Interpolator::Interpolator(){
  nmatrix_.newsize(4,4);
  Clear();

#ifdef BIAS_DEBUG
  SetDebugLevel(0);
  //  AddDebugLevel(D_INTERPOL_COMMON);
  //  AddDebugLevel(D_INTERPOL_SPLINE3);
#endif
}

Interpolator::~Interpolator(){
  Clear(); 
  nmatrix_.clear();
 
}

void Interpolator::Clear() {
  nmatrix_.SetZero();
  actIndex_=0;  // the index of the actual intervall for t
  listPolynoms1_.clear();
  listPolynoms2_.clear();
  listPolynoms3_.clear();
  KPts_.clear();
  T_.clear();
  listPntDim1_.clear();
  listPntDim2_.clear();
  listPntDim3_.clear();
  initializedPoly_=0;
  startTangentX_=-10000;
  startTangentY_=-10000;
  startTangentZ_=-10000;
  endTangentX_=-10000;
  endTangentY_=-10000;
  endTangentZ_=-10000;
  bXK_1_.clear();
  bXK_2_.clear();
  bXK_3_.clear();
  bYK_1_.clear();
  bYK_2_.clear();
  bYK_3_.clear();
  bezierInitialized_ = false;
  LuTMin1_ = -10000;
  LuTMax1_ = -10000;
  LuTSampleDist1_ = 0;
  SplineLuT1_.clear();
  listTangDim1_.clear();
}

void
Interpolator::SetControlPoints(const std::vector<Vector2<double> > &cPnt)
{
  double temp;
  unsigned n = cPnt.size();
  if(n==0) return;

  listPntDim1_.clear();listPntDim1_.resize(n);
  listPntDim2_.clear();listPntDim2_.resize(n);
  
  for (unsigned int i=0;i<n;i++) {
    temp=cPnt[i][0];listPntDim1_[i] = temp;
    temp=cPnt[i][1];listPntDim2_[i] = temp;
  }
}

void
Interpolator::
SetControlPoints(const std::vector<BIAS::Vector3<double> > &cPnt) {
  double temp;
  unsigned n = cPnt.size();
  if(n==0) return;
  listPntDim1_.clear();listPntDim1_.resize(n);
  listPntDim2_.clear();listPntDim2_.resize(n);
  listPntDim3_.clear();listPntDim3_.resize(n);

  for (unsigned int i=0;i<n;i++) {
    temp=cPnt[i][0];listPntDim1_[i] = temp;
    temp=cPnt[i][1];listPntDim2_[i] = temp;
    temp=cPnt[i][2];listPntDim3_[i] = temp;
  }
}

void
Interpolator::GetControlPoints(std::vector<Vector2<double> > &cPnt) const
{
  Vector2<double> temp;
  cPnt.clear();
  for (unsigned int i=0;i<listPntDim1_.size();i++) {
    temp[0]=listPntDim1_[i];
    temp[1]=listPntDim2_[i];
    cPnt.push_back(temp);
  }
}

void
Interpolator::
GetControlPoints(std::vector<BIAS::Vector3<double> > &cPnt) const
{
  Vector3<double> temp;
  cPnt.clear();
  for (unsigned int i=0;i<listPntDim1_.size();i++) {
    temp[0]=listPntDim1_[i];
    temp[1]=listPntDim2_[i];
    temp[2]=listPntDim3_[i];
    cPnt.push_back(temp);
  }
}

bool Interpolator::validate(double &t, bool debug)
{
  BIASDOUT(D_INTERPOL_COMMON,"validating t:"<<t);
  if (listPntDim1_.size()>KPts_.size()) { // make it uniform
    KPts_.clear();
    T_.clear();
    for (unsigned int i=0;i<listPntDim1_.size();i++)
      KPts_.push_back((double)i/(double)(listPntDim1_.size()-1));
      T_.push_back(1);
  }
  unsigned int tSize=KPts_.size()-1;
  if ( (tSize<=0) || (listPntDim1_.size()<=1) ) {    
    BIASERR("less than 2 control points");
    return false;
  }
  if (t<=KPts_[0]) {t=KPts_[0]; actIndex_=0; return true;}

  // find the correct intervall
  unsigned int i=0;
  while ((i<tSize) && (KPts_[i]<t))
    i++;
  actIndex_=i-1;

  if(debug && actIndex_ < 0){
    cout<<"t:"<<t<<endl;
    cout<<"ActIndex_ = "<<actIndex_<<endl;
  }

  if(actIndex_<0)
    return false;
  else
    return true;
}

void Interpolator::SetKnotPoints(const std::vector<double> &kPnt){
  unsigned int n=0;
  if ((n=kPnt.size())==0) return;
  //clear before resize
  KPts_.clear();
  T_.clear();

  KPts_.resize(n);
  T_.resize(n);
  for (unsigned int i=0;i<n;i++)
    KPts_[i]=kPnt[i];
  for (unsigned int i=1;i<n;i++)
    T_[i]=kPnt[i]-kPnt[i-1];
}

void Interpolator::InitSpline()
{
  initializedPoly_=0;
}


void Interpolator::InitSplineAkima(std::vector<BIAS::Vector<double> > &listP,
                                   const std::vector<double>& listPnt,
                                   const double startTangent, const double endTangent)
{
  int n=listPnt.size();
  BIASDOUT(D_INTERPOL_SPLINE3,"n="<<n);
  if (n<5) {
    InitSpline2(listP,listPnt,-10000);
    return;
  }

  BIAS::Vector<double> a(4); // the polynom coefficients
  BIAS::Vector<double> Px(6);
  BIAS::Vector<double> Py(6);
  int Min, Max;
  int from, to;

  for (int j=0; j<n-1; j++) {
    from = 0;
    to = 5;

    Min = j - 2;
    if (Min<0) {
      if (startTangent==-10000) {
        from -= Min;
      }
      Min = 0;
    }
    Max = j + 3;
    if (Max>n-1) {
      if (endTangent==-10000) {
        to += n-1 - Max;
      }
      Max = n-1;
    }

    int k = 0;
    for (int i=j-2; i<=j+3; i++) {
      if (i>=Min && i<=Max) {
        Px[k] = KPts_[i];
        Py[k] = listPnt[i];
      } else {
        if (i<Min) {
          if (startTangent==-10000) {
            Px[k] = KPts_[Min];
            Py[k] = listPnt[Min];
          } else {
            Px[k] = KPts_[Min] - (KPts_[Min+1]-KPts_[Min]) * (Min-i);
            Py[k] = listPnt[Min] -
              (KPts_[Min+1]-KPts_[Min]) * (Min-i) * startTangent;
          }
        } else {
          if (endTangent==-10000) {
            Px[k] = KPts_[Max];
            Py[k] = listPnt[Max];
          } else {
            Px[k] = KPts_[Max] + (KPts_[Max]-KPts_[Max-1]) * (i-Max);
            Py[k] = listPnt[Max] +
              (KPts_[Max]-KPts_[Max-1]) * (i-Max) * endTangent;
          }
        }
      }
      k++;
    }
    double xx=Px[2];
    for (int i=0; i<6; i++) {
      Px[i]-=xx;
    }
    fakima(a, Px, Py, from, to, 2);
    listP.push_back(a);
    }
  initializedPoly_=4;
}


void Interpolator::fakima(BIAS::Vector<double> &a,
                          BIAS::Vector<double> Px, BIAS::Vector<double> Py,
                          int from, int to, int intervall)
{
    double d1,d2;
    double a1,a2;
  BIAS::Vector<double> dq(6);
  BIAS::Vector<double> si(2);
  BIAS::Matrix<double> A(4,4);
  BIAS::Vector<double> b(4);

    for (int i=0; i<from; i++)  {
        d1 = Py[from+1] - Py[from];
        d2 = Px[from+1] - Px[from];
        if (d2 == 0)
            dq[i] = 0.0;
        else
            dq[i] = d1/d2;
   }
    for (int i=from; i<to; i++) {
        d1 = Py[i+1] - Py[i];
        d2 = Px[i+1] - Px[i];
        if (d2 == 0)
            dq[i] = 0.0;
        else
            dq[i] = d1/d2;
    }
    for (int i=to; i<6; i++)    {
        d1 = Py[to] - Py[to-1];
        d2 = Px[to] - Px[to-1];
        if (d2 == 0)
            dq[i] = 0.0;
        else
            dq[i] = d1/d2;
   }

    // calculate tangent si0
  double eps=0.000001;
    a1 = fabs(dq[intervall+1] - dq[intervall]);
    a2 = fabs(dq[intervall-1] - dq[intervall-2]);
    if ((a1+a2) < eps) {
        si[0] = (dq[intervall-1]+dq[intervall]) / 2;
    } else {
        si[0] = (a1 * dq[intervall-1] + a2 * dq[intervall]) / (a1+a2);
  }

    // calculate tangent si1
    intervall ++;
    a1 = fabs(dq[intervall+1] - dq[intervall]);
    a2 = fabs(dq[intervall-1] - dq[intervall-2]);
    if ((a1+a2) <eps) {
        si[1] = (dq[intervall-1]+dq[intervall]) / 2;
  } else {
        si[1] = (a1 * dq[intervall-1] + a2 * dq[intervall]) / (a1+a2);
  }

  //construct LGS
    intervall --;
    A[0][0] = ((double)(Px[intervall])) *
    ((double)(Px[intervall])) * ((double)(Px[intervall]));
    A[0][1] = ((double)(Px[intervall])) * ((double)(Px[intervall]));
    A[0][2] = (double)(Px[intervall]);
    A[0][3] = 1.0;

    A[1][0] = ((double)(Px[intervall+1])) *
    ((double)(Px[intervall+1])) * ((double)(Px[intervall+1]));
    A[1][1] = ((double)(Px[intervall+1])) * ((double)(Px[intervall+1]));
    A[1][2] = (double)(Px[intervall+1]);
    A[1][3] = 1.0;

    A[2][0] = 3.0 * ((double)(Px[intervall])) * ((double)(Px[intervall]));
    A[2][1] = 2.0 * ((double)(Px[intervall]));
    A[2][2] = 1.0;
    A[2][3] = 0.0;

    A[3][0] = 3.0 * ((double)(Px[intervall+1])) * ((double)(Px[intervall+1]));
    A[3][1] = 2.0 * ((double)(Px[intervall+1]));
    A[3][2] = 1.0;
    A[3][3] = 0.0;

    b[0] = (double)(Py[intervall]);
    b[1] = (double)(Py[intervall+1]);
    b[2] = si[0];
    b[3] = si[1];

  a=Lapack_LU_linear_solve(A,b);
}

void Interpolator::
InitSplineHermite(std::vector<double> &tangents,
                  const std::vector<double> &listPnt,
                  const double startTangent, const double endTangent)
{
  unsigned int n=listPnt.size();
  BIASDOUT(D_INTERPOL_SPLINE3,"n="<<n);

  tangents.resize(n);

  if (startTangent == -10000) {
    tangents[0] = 0.0f;
  } else {
    tangents[0] = startTangent;
  }
  if (endTangent == -10000) {
    tangents[n-1] = 0.0f;
  } else {
    tangents[n-1] = endTangent;
  }

  for (unsigned int i=1;i<n-1;i++) {
    double s1 = (listPnt[i+1] - listPnt[i]) / (2* (KPts_[i+1] - KPts_[i]));
    double s2 = (listPnt[i] - listPnt[i-1]) / (2* (KPts_[i] - KPts_[i-1]));
    tangents[i] = (s1 + s2);
  }


  initializedPoly_=5;
}


void
Interpolator::
InitSpline3(std::vector<BIAS::Vector<double> > &listP,
                  const std::vector<double> &listPnt,
                  const double startTangent, const double endTangent)
{
  // t0 is 0!   different t values are handed by tOffset_
  unsigned int i;
  unsigned int n=listPnt.size();
  BIASDOUT(D_INTERPOL_SPLINE3,"n="<<n);
  if (n==3) {
    InitSpline2(listP,listPnt,startTangent);
    return;
  }
  // first calculate the tangents for all control points
  // x is the solution with the tangent values
  // if start or end tangent are not set, thenthe relaxed end condition
  // P1''=0 or Pn''=0 is used
  int stAdd=0;
  if (startTangent==-10000) {
    stAdd=1;
  }
  int edAdd=0;
  if (endTangent==-10000) {
    edAdd=1;
  }
  BIAS::Vector<double> x(n-2+stAdd+edAdd),b(n-2+stAdd+edAdd);
  assert (n>2);
  BIAS::Matrix<double> A(n-2+stAdd+edAdd,n-2+stAdd+edAdd);
  A.Fill(0);

  // linear equations like those in
  // David Salomon's CG, page 238 for nonuniform Splines
  if (stAdd==0) {
    // startTangent set
    A[0][0]=2.0*(T_[1] + T_[2]);
    A[0][1]=T_[1];
    b[0]= 3.0/(T_[1]*T_[2]) *
      ( T_[1]*T_[1] * (listPnt[2] - listPnt[1]) +
        T_[2]*T_[2] * (listPnt[1] - listPnt[0]) ) -
      T_[2] * startTangent;
  } else {
    // relaxed start condition
    A[0][0]=2.0*T_[1];
    A[0][1]=T_[1];
    b[0]= 3.0 * (listPnt[1] - listPnt[0]);
  }
  for (i=1;i<n-3+stAdd+edAdd;i++) {
    A[i][i-1]=T_[i+2-stAdd];
    A[i][i] = 2.0 * (T_[i+1-stAdd] + T_[i+2-stAdd]);
    A[i][i+1] = T_[i+1-stAdd];
    b[i] = 3.0/(T_[i+1-stAdd]*T_[i+2-stAdd]) *
      ( T_[i+1-stAdd]*T_[i+1-stAdd] *
        (listPnt[i+2-stAdd] - listPnt[i+1-stAdd]) +
        T_[i+2-stAdd]*T_[i+2-stAdd] * (listPnt[i+1-stAdd] - listPnt[i-stAdd]));
  }
  if (edAdd==0) {
    // endTangent set
    A[n-3+stAdd][n-4+stAdd] = T_[n-1];
    A[n-3+stAdd][n-3+stAdd] = 2.0 * (T_[n-1] + T_[n-2]);
    b[n-3+stAdd]= 3.0/(T_[n-1]*T_[n-2]) *
      ( T_[n-2]*T_[n-2] * (listPnt[n-1] - listPnt[n-2]) +
        T_[n-1]*T_[n-1] * (listPnt[n-2] - listPnt[n-3]) ) -
      T_[n-2] * endTangent;
  } else {
    // relaxed end condition
    A[n-2+stAdd][n-3+stAdd] = T_[n-1];
    A[n-2+stAdd][n-2+stAdd] = 2.0 * T_[n-1];
    b[n-2+stAdd]= 3.0 * (listPnt[n-1] - listPnt[n-2]);
  }
  BIASDOUT(D_INTERPOL_SPLINE3," A:"<<A);

  x=Lapack_LU_linear_solve(A,b);
  if(x.size()==0){
    BIASERR("Lapack_LU_Linear_solve failed!");
    BIAS::SVD svdA;
    int res = svdA.Compute(A);
    if(res < 0){
      BIASERR("SVD failed! Aborting...");
      return;
    }
    x = svdA.Invert()*b;
  }
  BIASDOUT(D_INTERPOL_SPLINE3,"Vector x:"<<x);

  BIAS::Vector<double> a(4); // the polynom coefficients
  double t_i_1;
  if (stAdd==0) {
    // startTangent set
    // the first segment
    t_i_1= T_[1];
    a[0]= listPnt[0];
    a[1]= startTangent;
    a[2]= 3.0* (listPnt[1]-listPnt[0])/(t_i_1*t_i_1) -
      2.0/t_i_1* startTangent - x[0]/t_i_1;
    a[3]= 2.0/(t_i_1*t_i_1*t_i_1)* (listPnt[0]-listPnt[1]) +
      (startTangent + x[0]) / (t_i_1*t_i_1);
    listP.push_back(a);
  }
  BIASDOUT(D_INTERPOL_SPLINE3,"First Polynom:"<<a);
  for (i=1;i<n-2+stAdd+edAdd;i++) { // second segment to n-2 segment
    t_i_1= T_[i+1-stAdd];
    a[0]= listPnt[i-stAdd];
    a[1]= x[i-1]; // x are the tangent values
    a[2]= 3.0/(t_i_1*t_i_1)* (listPnt[i+1-stAdd]-listPnt[i-stAdd]) -
      2.0/t_i_1* x[i-1] - x[i]/t_i_1;
    a[3]= 2.0/(t_i_1*t_i_1*t_i_1)* (listPnt[i-stAdd]-listPnt[i+1-stAdd]) +
      (x[i-1] + x[i]) / (t_i_1*t_i_1);
    listP.push_back(a);
    BIASDOUT(D_INTERPOL_SPLINE3,"polynom "<<i<<": "<<a);
  }
  if (edAdd==0) {
    // endTangent set
    // last (n-1) segment
    t_i_1= T_[n-1];
    a[0]= listPnt[n-2];
    a[1]= x[n-3+stAdd];
    a[2]= 3.0/(t_i_1*t_i_1)* (listPnt[n-1]-listPnt[n-2]) -
      2.0/t_i_1* x[n-3+stAdd] - endTangent/t_i_1;
    a[3]= 2.0/(t_i_1*t_i_1*t_i_1)* (listPnt[n-2]-listPnt[n-1]) +
      (x[n-3+stAdd] + endTangent) / (t_i_1*t_i_1);
    listP.push_back(a);
  }
  initializedPoly_=3;

//    for (int i=0; i<n; i++) {
//      cout << i << "  T:" << T_[i] << "    listPnt:" << listPnt[i]
//           << endl;
//    }

//    cout << "ST x:" << startTangent << endl;
//    for (int i=0; i<n-2; i++) {
//      cout << i+1 << "  x:" << x[i] << endl;
//    }
//    cout << "ET x:" << endTangent << endl;
}

void
Interpolator::
InitSpline2(std::vector<BIAS::Vector<double> > &listP,
                  const std::vector<double>& listPnt,
                  double startTangent)
{
  unsigned int n=listPnt.size();
  if (n<3) {
    InitLinear(listP,listPnt);
    return;
  }
  unsigned int i;
  const double eps=0.00000001;
  BIAS::Vector<double> a(3);
  double t0,t1, y0,y1,s1;

  // take the next control point for each new polynom
  double t0Deriv;
  if (startTangent==-10000)
    startTangent = 0;

  for (i=0;i<n-1;i++) {
    t0=KPts_[i];t1=KPts_[i+1];
    y0=listPnt[i];y1=listPnt[i+1];
    if (i==0) t0Deriv=startTangent;
    else t0Deriv=a[1]+ 2*a[2] * t0;
    s1=2*t0*(t1-t0) - (t1*t1 - t0*t0);
    if (fabs(s1)<eps) a[2]=0;
    else a[2]=(t0Deriv*(t1-t0) - y1 + y0) / s1;
    a[1]=t0Deriv - 2*a[2]*t0;
    a[0]=y0 - a[2]*t0*t0 - a[1]*t0;
    listP.push_back(a);
    BIASDOUT(D_INTERPOL_SPLINE2,"t0Deriv:"<<t0Deriv<<" a:"<<a);
  }
  //  BIASDOUT(D_POLYNOM,"a0:"<<a[0]<<" a1:"<<a[1]<<" a2:"<<a[2]);
  //  cerr<<"a:"<<a;
  initializedPoly_=2;
}

void Interpolator::
InitLinear(std::vector<BIAS::Vector<double> > &listP,
           const std::vector<double>& listPnt)
{
  unsigned int i,n=listPnt.size()-1;
  BIAS::Vector<double> a(2);
  for (i=0;i<n;i++) {
    if (T_[i+1]>0)
      a[1]=(listPnt[i+1]-listPnt[i]) / T_[i+1];
    else a[1]=0;
    a[0]=listPnt[i]-a[1]*KPts_[i];
    listP.push_back(a);
  }
  initializedPoly_=1;
}


// calculates the polynom coefficients for each intervall
void Interpolator::
InitSpline(const unsigned int k, const unsigned int dim)
{
  listPolynoms1_.clear();
  listPolynoms2_.clear();
  listPolynoms3_.clear();
  actIndex_=0;
  if (dim==1) {
    if (k==1)
      InitLinear(listPolynoms1_,listPntDim1_);
    if (k==2)
      InitSpline2(listPolynoms1_,listPntDim1_,startTangentX_);
    if (k==3)
      InitSpline3(listPolynoms1_,listPntDim1_,startTangentX_,endTangentX_);
    if (k==4)
      InitSplineAkima(listPolynoms1_,listPntDim1_,startTangentX_,endTangentX_);
    if (k==5) {
      if (listTangDim1_.size() == listPntDim1_.size()) {
        initializedPoly_=5;
      } else {
        InitSplineHermite(listTangDim1_,listPntDim1_,startTangentX_,endTangentX_);
      }
    }
  }
  if (dim==2)  {
    if (k==1) {
      InitLinear(listPolynoms1_,listPntDim1_);
      InitLinear(listPolynoms2_,listPntDim2_);
    }
    if (k==2) {
      InitSpline2(listPolynoms1_,listPntDim1_,startTangentX_);
      InitSpline2(listPolynoms2_,listPntDim2_,startTangentY_);
    }
    if (k==3) {
      InitSpline3(listPolynoms1_,listPntDim1_,startTangentX_,endTangentX_);
      InitSpline3(listPolynoms2_,listPntDim2_,startTangentY_,endTangentY_);
    }
    if (k==4) {
      InitSplineAkima(listPolynoms1_,listPntDim1_,startTangentX_,endTangentX_);
      InitSplineAkima(listPolynoms2_,listPntDim2_,startTangentY_,endTangentY_);
    }
  }
  if (dim==3) {
    if (k==1) {
      InitLinear(listPolynoms1_,listPntDim1_);
      InitLinear(listPolynoms2_,listPntDim2_);
      InitLinear(listPolynoms3_,listPntDim3_);
    }
    if (k==2) {
      InitSpline2(listPolynoms1_,listPntDim1_,startTangentX_);
      InitSpline2(listPolynoms2_,listPntDim2_,startTangentY_);
      InitSpline2(listPolynoms3_,listPntDim3_,startTangentZ_);
    }
    if (k==3) {
      InitSpline3(listPolynoms1_,listPntDim1_,startTangentX_,endTangentX_);
      InitSpline3(listPolynoms2_,listPntDim2_,startTangentY_,endTangentY_);
      InitSpline3(listPolynoms3_,listPntDim3_,startTangentZ_,endTangentZ_);
    }
    if (k==4) {
      InitSplineAkima(listPolynoms1_,listPntDim1_,startTangentX_,endTangentX_);
      InitSplineAkima(listPolynoms2_,listPntDim2_,startTangentY_,endTangentY_);
      InitSplineAkima(listPolynoms3_,listPntDim3_,startTangentZ_,endTangentZ_);
    }
  }
}


int Interpolator::Spline(double &res, double t, unsigned int k)
{
  // if less than 4 control points and not hermite
  if (listPntDim1_.size()<=k && k < 5)
    k=listPntDim1_.size()-1;

  if (initializedPoly_!=k){
    //cout<<"reinit spline:"<<k<<endl;
    InitSpline(k,1);
  }

  if (!validate(t,true)){
    BIASERR("invalid values, result is undefined");
    res=0;
    return -1;
  }

  if (t>KPts_[KPts_.size()-1]) {
    res=listPntDim1_[actIndex_+1];
    return 1;
  }

  if (k==3 || k==4) {
    t = t-KPts_[actIndex_];
  }

  res=0;
  if (k!=5) {
    for (int i=0;i<listPolynoms1_[actIndex_].size();i++) {
      if (k!=4) {
        res+=listPolynoms1_[actIndex_][i]*pow(t,i);
      } else {
        res+=listPolynoms1_[actIndex_][i]*pow(t,3-i);
      }
    }
  } else {
    // hermite spline
    double h = KPts_[actIndex_+1] - KPts_[actIndex_];
    t = (t - KPts_[actIndex_]) / h;
    double t2 = t*t;
    double t3 = t*t2;
    double h00 = 2*t3 - 3*t2 + 1;
    double h10 = t3 - 2*t2 + t;
    double h01 = -2*t3 + 3*t2;
    double h11 = t3 - t2;
    res  = h00*listPntDim1_[actIndex_];
    res += h10*h*listTangDim1_[actIndex_];
    res += h01*listPntDim1_[actIndex_+1];
    res += h11*h*listTangDim1_[actIndex_+1];
  }
  return 0;
}


int Interpolator::Spline(Vector2<double> &res, double t, unsigned int k)
{
  if (!validate(t)) {
    BIASERR("invalid values, result is undefined");
    res = 0;
    return -1;
  }
  if (t>KPts_[KPts_.size()-1]) {
    res[0]=listPntDim1_[actIndex_+1];
    res[1]=listPntDim2_[actIndex_+1];
    return 1;
  }
   // if less than 4 control points
  if (listPntDim1_.size()<=k) k=listPntDim1_.size()-1;

  if (initializedPoly_!=k)
    InitSpline(k,2);
  res.Set(0);
  double t0=KPts_[actIndex_], t1= KPts_[actIndex_+1];
  double tint1= T_[actIndex_+1];
  if (k==3 || k==4) {
    t = (t-t0)/(t1-t0) * tint1;
  }
  for (int i=0;i<listPolynoms1_[actIndex_].size();i++) {
    res[0]+=listPolynoms1_[actIndex_][i]*pow(t,(int)i);
    res[1]+=listPolynoms2_[actIndex_][i]*pow(t,(int)i);
  }
  return 0;
}

int Interpolator::Spline(Vector3<double> &res, double t, unsigned int k)
{
  if (!validate(t)) {
    BIASERR("invalid values, result is undefined");
    res.Set(0.0);
    return -1;
  }
  BIASDOUT(D_INTERPOL_COMMON,"validate returned true");
  if (t>KPts_.back()) {
    res[0]=listPntDim1_[actIndex_+1];
    res[1]=listPntDim2_[actIndex_+1];
    res[2]=listPntDim3_[actIndex_+1];
    return 1;
  }
  // if less than 4 control points
  if (listPntDim1_.size()<=k) k=listPntDim1_.size()-1;

  if (initializedPoly_!=k)
    InitSpline(k,3);
  res.Set(0.0);
  double t0=KPts_[actIndex_], t1= KPts_[actIndex_+1];
  double tint1= T_[actIndex_+1];
  if (k==3 || k==4) {
    t = (t-t0)/(t1-t0) * tint1;
  }
  BIASDOUT(D_INTERPOL_COMMON,"evaluating result");
  for (int i=0;i<listPolynoms1_[actIndex_].size();i++) {
    res[0]+=listPolynoms1_[actIndex_][i]*pow(t,(int)i);
    res[1]+=listPolynoms2_[actIndex_][i]*pow(t,(int)i);
    res[2]+=listPolynoms3_[actIndex_][i]*pow(t,(int)i);
  }
  return 0;
}

/**
    Calculation of the coefficients of the Bezier interpolation
    @author Ingo Schiller
    @param dimension of control points (int)
    @status tested 12.05.03
*/

void
Interpolator::InitBezier(int dim_of_CP)
{
  //n = number of control points
  int n = listPntDim1_.size();
  bezierInitialized_ = true;
  BIASDOUT(D_INTERPOL_BEZIER,"Number of CPs: "<<n);

  double stTanX, stTanY, stTanZ;
  double edTanX, edTanY, edTanZ;
  if (startTangentX_==-10000) {
    stTanX = 0;
  } else {
    stTanX = startTangentX_;
  }
  if (startTangentY_==-10000) {
    stTanY = 0;
  } else {
    stTanY = startTangentY_;
  }
  if (startTangentZ_==-10000) {
    stTanZ = 0;
  } else {
    stTanZ = startTangentZ_;
  }
  if (endTangentX_==-10000) {
    edTanX = 0;
  } else {
    edTanX = endTangentX_;
  }
  if (endTangentY_==-10000) {
    edTanY = 0;
  } else {
    edTanY = endTangentY_;
  }
  if (endTangentZ_==-10000) {
    edTanZ = 0;
  } else {
    edTanZ = endTangentZ_;
  }
  //set the matrix(row,column)
  nmatrix_.Fill(0.0);
  nmatrix_[0][0]=-1;
  nmatrix_[1][0]=nmatrix_[0][1]=nmatrix_[1][2]=nmatrix_[2][1]=3;
  nmatrix_[2][0]=nmatrix_[0][2]=-3;
  nmatrix_[1][1]=-6;
  nmatrix_[3][0]=nmatrix_[0][3]=1;

  BIASDOUT(D_INTERPOL_BEZIER,"NMatrix set");

  switch(dim_of_CP){
  case 1:
    bXK_1_.clear();
    bYK_1_.clear();
    bXK_1_.reserve(n-1);
    bYK_1_.reserve(n-1);


    //set X0 according to start- and endtangents
    bXK_1_.push_back(listPntDim1_[0]+stTanX);
    bYK_1_.push_back(0.0);

    // rest of coefficients, Xn not requiered,  Y0 not requiered
    for(unsigned int i =1;i<listPntDim1_.size()-1;i++){
      bXK_1_.push_back(listPntDim1_[i]+(listPntDim1_[i+1]-listPntDim1_[i-1])/2);
      bYK_1_.push_back(listPntDim1_[i]-(listPntDim1_[i+1]-listPntDim1_[i-1])/2);
    }

    //Yn according to start- and endtangents
    bYK_1_.push_back(listPntDim1_.back()-edTanX);
    //Xn to keep the size
    bXK_1_.push_back(0.0);
    break;

  case 2:
    bXK_1_.clear();
    bXK_2_.clear();
    bYK_1_.clear();
    bYK_2_.clear();

    bXK_1_.reserve(n-1);
    bXK_2_.reserve(n-1);
    bYK_1_.reserve(n-1);
    bYK_2_.reserve(n-1);

    //set X0 according to start- and endtangents
    bXK_1_.push_back(listPntDim1_[0]+stTanX);
    bXK_2_.push_back(listPntDim2_[0]+stTanY);
    bYK_1_.push_back(0.0);
    bYK_2_.push_back(0.0);

    // rest of coefficients, Xn not requiered,Y0 not requiered
    for(unsigned int i =1;i<listPntDim1_.size()-1;i++){
      bXK_1_.push_back(listPntDim1_[i]+(listPntDim1_[i+1]-listPntDim1_[i-1])/2);
      bXK_2_.push_back(listPntDim2_[i]+(listPntDim2_[i+1]-listPntDim2_[i-1])/2);
      bYK_1_.push_back(listPntDim1_[i]-(listPntDim1_[i+1]-listPntDim1_[i-1])/2);
      bYK_2_.push_back(listPntDim2_[i]-(listPntDim2_[i+1]-listPntDim2_[i-1])/2);
    }

    //set Yn according to start tangents
    bYK_1_.push_back(listPntDim1_.back()-edTanX);
    bYK_2_.push_back(listPntDim2_.back()-edTanY);
    //set Xn to keep track of the size
    bXK_1_.push_back(0.0);
    bXK_2_.push_back(0.0);

    BIASDOUT(D_INTERPOL_BEZIER,"Alle koeffizienten berechnet");
    break;


  case 3:
    bXK_1_.clear();
    bXK_2_.clear();
    bXK_3_.clear();

    bYK_1_.clear();
    bYK_2_.clear();
    bYK_3_.clear();

    bXK_1_.reserve(n-1);
    bXK_2_.reserve(n-1);
    bXK_3_.reserve(n-1);
    bYK_1_.reserve(n-1);
    bYK_2_.reserve(n-1);
    bYK_3_.reserve(n-1);

    BIASDOUT(D_INTERPOL_BEZIER,"bXK and bYK resized.");

 //set X0 according to start- and endtangents
    bXK_1_.push_back(listPntDim1_[0]+stTanX);
    bXK_2_.push_back(listPntDim2_[0]+stTanY);
    bXK_3_.push_back(listPntDim3_[0]+stTanZ);

    bYK_1_.push_back(0.0);
    bYK_2_.push_back(0.0);
    bYK_3_.push_back(0.0);

    // rest of coefficients, Xn not requiered Y0 not requiered
    for(unsigned int i =1;i<listPntDim1_.size()-1;i++){
      bXK_1_.push_back(listPntDim1_[i]+(listPntDim1_[i+1]-listPntDim1_[i-1])/2);
      bXK_2_.push_back(listPntDim2_[i]+(listPntDim2_[i+1]-listPntDim2_[i-1])/2);
      bXK_3_.push_back(listPntDim3_[i]+(listPntDim3_[i+1]-listPntDim3_[i-1])/2);
      bYK_1_.push_back(listPntDim1_[i]-(listPntDim1_[i+1]-listPntDim1_[i-1])/2);
      bYK_2_.push_back(listPntDim2_[i]-(listPntDim2_[i+1]-listPntDim2_[i-1])/2);
      bYK_3_.push_back(listPntDim3_[i]-(listPntDim3_[i+1]-listPntDim3_[i-1])/2);
    }
    //set Yn according to start- and endtangents
    bYK_1_.push_back(listPntDim1_.back()-edTanX);
    bYK_2_.push_back(listPntDim2_.back()-edTanY);
    bYK_3_.push_back(listPntDim3_.back()-edTanZ);

    // set Xn to keep track of the size
    bXK_1_.push_back(0.0);
    bXK_2_.push_back(0.0);
    bXK_3_.push_back(0.0);

    BIASDOUT(D_INTERPOL_BEZIER,"Alle koeffizienten berechnet");

    break;
  default:
    cout<<"WRONG DIMENSION IN BEZIER INTERPOLATION"<<endl;
    break;
  }
}

/**
    1 dimensional case
    @status untested 12.05.03
    @author Ingo Schiller
*/
int Interpolator::Bezier3(double &res, double t)
{
  if(!bezierInitialized_)
    InitBezier(1);

#ifdef BIAS_DEBUG
  int n = listPntDim1_.size(); //= number of control points
  int segments = n-1;  // number of segments
#endif
  int in_segment = 0; //in which segment is t?

  //tvalues
  BIAS::Matrix<double> tvector(1,4);
  // the used Control points are stored here
  BIAS::Matrix<double> cpoints(4,1,0.0);


  /**  detection in which segment t is found starting with a segment 0
   */
  if (!validate(t)) {
    BIASERR("invalid values, result is undefined");
    res=0;
    return -1;
  }
  in_segment = actIndex_;

  //if t > limit return the value of last control point
  if(t>KPts_.back()) {
    res=listPntDim1_.back();
    return 1;
  }

  // now store the valid points in cpoints
  //Pi
  cpoints[0][0]= listPntDim1_[in_segment];
  //Xi
  cpoints[1][0]= bXK_1_[in_segment];
  //Yi+1
  cpoints[2][0]= bYK_1_[in_segment+1];
  //Pi+1
  cpoints[3][0]= listPntDim1_[in_segment+1];


  //now adjust t according to number of segments
  t=(t-KPts_[in_segment])/(KPts_[in_segment+1]-KPts_[in_segment]);

  BIASDOUT(D_INTERPOL_BEZIER," t:"<<t<<" Segs:"<<segments);
  BIASDOUT(D_INTERPOL_BEZIER,"In Segment:"<<in_segment);

//set the tvector
  tvector[0][3] = 1;
  tvector[0][2] = t;
  tvector[0][1] = pow(t,2);
  tvector[0][0] = pow(t,3);

  BIASDOUT(D_INTERPOL_BEZIER,"The TVector"<<tvector);
  BIASDOUT(D_INTERPOL_BEZIER,"The NMatrix"<<nmatrix_);
  BIASDOUT(D_INTERPOL_BEZIER,"The PMatrix"<<cpoints);

//do the Matrix multiplikations
  tvector.Mult(nmatrix_);
  BIASDOUT(D_INTERPOL_BEZIER,"tvector mult nmatrix.");
  tvector.Mult(cpoints);
  BIASDOUT(D_INTERPOL_BEZIER,"tvector mult cpoints.");

  res = tvector[0][0];

  BIASDOUT(D_INTERPOL_BEZIER,"Result : "<<res);

  return 0;
}

/**
   2 dimensional case
   @status tested 28.04.03
   @author Ingo Schiller
 */

int Interpolator::Bezier3(BIAS::Vector2<double> &res, double t)
{
  if(!bezierInitialized_)
    InitBezier(2);

#ifdef BIAS_DEBUG
  int n = listPntDim1_.size(); //= number of control points
  int segments = n-1;  // number of segments
#endif
  int in_segment = 0; //in which segment is t?

  //tvalues
  BIAS::Matrix<double> tvector(1,4);
  // the used Control points are stored here
  BIAS::Matrix<double> cpoints(4,2,0.0);


 /**  detection in which segment t is found starting with a segment 0
   */
  if (!validate(t)) {
    BIASERR("invalid values, result is undefined");
    res[0]=0;
    res[1]=0;
    in_segment = 0;
    return -1;
  }
  else
    in_segment = actIndex_;

  //if t > limit, return the value of the last control point
 if(t>KPts_.back()){
    res[0]=listPntDim1_.back();
    res[1]=listPntDim2_.back();
    return 1;
  }

  // now store the valid points in cpoints
  //Pi
  cpoints[0][0]=listPntDim1_[in_segment];
  cpoints[0][1]=listPntDim2_[in_segment];
  //Xi
  cpoints[1][0]= bXK_1_[in_segment];
  cpoints[1][1]= bXK_2_[in_segment];
  //Yi+1
  cpoints[2][0]= bYK_1_[in_segment+1];
  cpoints[2][1]= bYK_2_[in_segment+1];
  //Pi+1
  cpoints[3][0]= listPntDim1_[in_segment+1];
  cpoints[3][1]= listPntDim2_[in_segment+1];

  //now adjust t according to number of segments
  t=(t-KPts_[in_segment])/(KPts_[in_segment+1]-KPts_[in_segment]);

  BIASDOUT(D_INTERPOL_BEZIER," t:"<<t<<" Segs:"<<segments);
  BIASDOUT(D_INTERPOL_BEZIER,"In Segment:"<<in_segment);

//set the tvectorbXK_2_.size()
  tvector[0][3] = 1;
  tvector[0][2] = t;
  tvector[0][1] = pow(t,2);
  tvector[0][0] = pow(t,3);

  BIASDOUT(D_INTERPOL_BEZIER,"The TVector"<<tvector);
  BIASDOUT(D_INTERPOL_BEZIER,"The NMatrix"<<nmatrix_);
  BIASDOUT(D_INTERPOL_BEZIER,"The PMatrix"<<cpoints);

//do the Matrix multiplikations
  tvector.Mult(nmatrix_);
  BIASDOUT(D_INTERPOL_BEZIER,"tvector mult nmatrix.");
  tvector.Mult(cpoints);
  BIASDOUT(D_INTERPOL_BEZIER,"tvector mult cpoints.");


  res[0] = tvector[0][0];
  res[1] = tvector[0][1];

  BIASDOUT(D_INTERPOL_BEZIER,"Result : "<<res[0]<<"  "<<res[1]);

  return 0;
}


/**
   3 dimensional case
   @status 28.04.03 tested and working
   @author Ingo Schiller
 */
int Interpolator::Bezier3(BIAS::Vector3<double> &res, double t)
{
  if(!bezierInitialized_)
    InitBezier(3);

#ifdef BIAS_DEBUG
  int n = listPntDim1_.size(); //= number of control points
  int segments = n-1;  // number of segments
#endif
  int in_segment = 0; //in which segment is t?

  //tvalues
  BIAS::Matrix<double> tvector(1,4);
  // the used Control points are stored here
  BIAS::Matrix<double> cpoints(4,3,0.0);

  /**  detection in which segment t is found starting with a segment 0
   */
  if (!validate(t)) {
    BIASERR("invalid values, result is undefined");
    res[0] = 0;
    res[1] = 0;
    res[2] = 0;

    return -1;
  }
  in_segment = actIndex_;

  //if t > last control point return value of last control point.
  if (t>KPts_.back()){
    res[0]=listPntDim1_.back();
    res[1]=listPntDim2_.back();
    res[2]=listPntDim3_.back();
    return 1;
  }

  // now store the valid points in cpoints
  //Pi
  cpoints[0][0]=listPntDim1_[in_segment];
  cpoints[0][1]=listPntDim2_[in_segment];
  cpoints[0][2]=listPntDim3_[in_segment];
  //Xi
  cpoints[1][0]= bXK_1_[in_segment];
  cpoints[1][1]= bXK_2_[in_segment];
  cpoints[1][2]= bXK_3_[in_segment];
  //Yi+1
  cpoints[2][0]= bYK_1_[in_segment+1];
  cpoints[2][1]= bYK_2_[in_segment+1];
  cpoints[2][2]= bYK_3_[in_segment+1];
  //Pi+1
  cpoints[3][0]= listPntDim1_[in_segment+1];
  cpoints[3][1]= listPntDim2_[in_segment+1];
  cpoints[3][2]= listPntDim3_[in_segment+1];

  //now adjust t according to number of segments
  t=(t-KPts_[in_segment])/(KPts_[in_segment+1]-KPts_[in_segment]);

  BIASDOUT(D_INTERPOL_BEZIER," t:"<<t<<" Segs:"<<segments);
  BIASDOUT(D_INTERPOL_BEZIER,"In Segment:"<<in_segment);

//set the tvector
  tvector[0][3] = 1;
  tvector[0][2] = t;
  tvector[0][1] = pow(t,2);
  tvector[0][0] = pow(t,3);


  BIASDOUT(D_INTERPOL_BEZIER,"The TVector"<<tvector);
  BIASDOUT(D_INTERPOL_BEZIER,"The NMatrix"<<nmatrix_);
  BIASDOUT(D_INTERPOL_BEZIER,"The PMatrix"<<cpoints);

//do the Matrix multiplikations
  tvector.Mult(nmatrix_);
  BIASDOUT(D_INTERPOL_BEZIER,"tvector mult nmatrix.");
  tvector.Mult(cpoints);
  BIASDOUT(D_INTERPOL_BEZIER,"tvector mult cpoints.");

  res[0] = tvector[0][0];
  res[1] = tvector[0][1];
  res[2] = tvector[0][2];
  BIASDOUT(D_INTERPOL_BEZIER,"Result :"<<res[0]<<" "<<res[1]<<" "<<res[2]);

  return 0;

}


const bool Interpolator::
DoLuTSplinesDiffer(const Interpolator& I) const
{
  if(LuTMin1_ != I.LuTMin1_ || LuTMax1_ != I.LuTMax1_ ||
     LuTSampleDist1_ != I.LuTSampleDist1_)
    return true;

  unsigned int N = SplineLuT1_.size();
  unsigned int Ncompare = I.SplineLuT1_.size();
  if(N != Ncompare || Ncompare <1 || N <1)
    return true;

  bool theyDiffer = false;
  for(unsigned int i = 0; i<N && !theyDiffer; i++ ) {
    theyDiffer = (SplineLuT1_[i] != I.SplineLuT1_[i]);
  }
  return theyDiffer;
}


int Interpolator::PrepareLuTSpline(double min, double max, unsigned int N,
                   unsigned int k)
{

  SplineLuT1_.resize(N);
  LuTMin1_ = min;
  LuTMax1_ = max;
  LuTSampleDist1_ = (max-min) / double(N-1);

  for (unsigned int i=0;i<N; i++)
    Spline(SplineLuT1_[i], min +  double(i) * LuTSampleDist1_, k);

//   for (unsigned int i=0;i<N; i++)
//     cout <<"LuT "<<i<<" : "<<min + double(i) * LuTSampleDist1_<<" -> "
//   <<SplineLuT1_[i]<<endl;

  return 0;
}


int Interpolator::SplineFromLuT(double &res, double t) const
{
  if (t<LuTMin1_ || t>LuTMax1_ || SplineLuT1_.size() == 0) return -1;
  int index = (int)rint((t - LuTMin1_) / LuTSampleDist1_);
  //make sure we are within the bounds
  if(index < 0) 
    index =0; 
  if(index > (int)SplineLuT1_.size()-1 )
    index=(int)SplineLuT1_.size()-1;
    
  BIASASSERT(index >= 0);
  BIASASSERT(index < (int)SplineLuT1_.size());
  res = SplineLuT1_[index];
  //cout <<"t: "<<t<<" index: "<<index<<" res: "<<res<<endl;
  return 0;
}