source: orxonox.OLD/trunk/src/lib/math/spline.cc

/*=========================================================================

  Program:   Visualization Toolkit
  Module:    $RCSfile: Spline.cxx,v $

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/
#include "spline.h"


//----------------------------------------------------------------------------
// Construct a spline wth the folloing defaults:
// ClampValueOff
Spline::Spline ()
{
  this->ComputeTime = 0;
  this->ClampValue = 0;
  this->PiecewiseFunction = vtkPiecewiseFunction::New();
  this->Intervals = NULL;
  this->Coefficients = NULL;
  this->LeftConstraint = 1;
  this->LeftValue = 0.0;
  this->RightConstraint = 1;
  this->RightValue = 0.0;
  this->Closed = 0;

  this->ParametricRange[0] = -1;
  this->ParametricRange[1] = -1;
}

//----------------------------------------------------------------------------
Spline::~Spline ()
{
  this->PiecewiseFunction->Delete();
  if (this->Coefficients)
  {
    delete [] this->Coefficients;
  }
  if (this->Intervals)
  {
    delete [] this->Intervals;
  }
}

//----------------------------------------------------------------------------
void Spline::SetParametricRange(double tMin, double tMax)
{
  if ( tMin != this->ParametricRange[0] || tMax != this->ParametricRange[1] )
  {
    if ( tMin >= tMax )
    {
      tMax = tMin + 1;
    }

    this->ParametricRange[0] = tMin;
    this->ParametricRange[1] = tMax;

    this->Modified();
  }
}

//----------------------------------------------------------------------------
void Spline::GetParametricRange(double tRange[2]) const
{
  if ( this->ParametricRange[0] != this->ParametricRange[1] )
  {
    tRange[0] = this->ParametricRange[0];
    tRange[1] = this->ParametricRange[1];
  }
  else
  {
    tRange[0] = this->PiecewiseFunction->GetRange()[0];
    tRange[1] = this->PiecewiseFunction->GetRange()[1];
  }
}

//----------------------------------------------------------------------------
double Spline::ComputeLeftDerivative()
{
  double *dptr = this->PiecewiseFunction->GetDataPointer();
  int size = this->PiecewiseFunction->GetSize();
  if ( dptr == NULL || size < 2 )
  {
    return 0.0;
  }
  else
  {
    return (dptr[2]-dptr[0]);
  }
}

//----------------------------------------------------------------------------
double Spline::ComputeRightDerivative()
{
  double *dptr = this->PiecewiseFunction->GetDataPointer();
  int size = this->PiecewiseFunction->GetSize();
  if ( dptr == NULL || size < 2 )
  {
    return 0.0;
  }
  else
  {
    return (dptr[(size-1)*2]-dptr[(size-2)*2]);
  }
}

//----------------------------------------------------------------------------
int Spline::GetNumberOfPoints()
{
  return this->PiecewiseFunction->GetSize();
}


//----------------------------------------------------------------------------
// Add a point to the Piecewise Functions containing the data
void Spline::AddPoint (double t, double x)
{
  if ( this->ParametricRange[0] != this->ParametricRange[1] )
  {
    t = (t < this->ParametricRange[0] ? this->ParametricRange[0] :
         (t > this->ParametricRange[1] ? this->ParametricRange[1] : t));
  }
  this->PiecewiseFunction->AddPoint (t, x);
}

//----------------------------------------------------------------------------
// Remove a point from the Piecewise Functions.
void Spline::RemovePoint (double t)
{
  if ( this->ParametricRange[0] != this->ParametricRange[1] )
  {
    t = (t < this->ParametricRange[0] ? this->ParametricRange[0] :
         (t > this->ParametricRange[1] ? this->ParametricRange[1] : t));
  }
  this->PiecewiseFunction->RemovePoint (t);
}

//----------------------------------------------------------------------------
// Remove all points from the Piecewise Functions.
void Spline::RemoveAllPoints ()
{
  this->PiecewiseFunction->RemoveAllPoints ();
}





//----------------------------------------------------------------------------
int Spline::FindIndex(int size, double t)
{
  int index=0;
  if ( size > 2 ) //bisection method for speed
  {
    int rightIdx = size - 1;
    int centerIdx = rightIdx - size/2;
    for (int converged=0; !converged; )
    {
      if ( this->Intervals[index] <= t && t <= this->Intervals[centerIdx] )
      {
        rightIdx = centerIdx;
      }
      else //if ( this->Intervals[centerIdx] < t && t <= this->Intervals[rightIdx] )
      {
        index = centerIdx;
      }
      if ( (index + 1) == rightIdx )
      {
        converged = 1;
      }
      else
      {
        centerIdx = index + (rightIdx-index)/2;
      }
    }//while not converged
  }
  return index;
}