src/vamos/geometry/Spline.cc

Go to the documentation of this file.

// Spline.cc - a cubic spline interpolator.
//
//  Vamos Automotive Simulator
//  Copyright (C) 2001--2004 Sam Varner
//
//  This program is free software; you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation; either version 2 of the License, or
//  (at your option) any later version.
//
//  This program 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 General Public License for more details.
//
//  You should have received a copy of the GNU General Public License
//  along with this program; if not, write to the Free Software
//  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

#include <vamos/geometry/Spline.h>

#include <cmath>
#include <cassert>

// Construct an empty curve.
Vamos_Geometry::
Spline::Spline (double first_slope, double last_slope) :
  m_first_slope (first_slope),
  m_last_slope (last_slope),
  m_calculated (false),
  m_slope (0.0)
{
}

// Construct a cuvre from an array of points.
Vamos_Geometry::
Spline::Spline (const std::vector <Two_Point>& points,
                double first_slope, double last_slope) :
  m_first_slope (first_slope),
  m_last_slope (last_slope),
  m_calculated (false),
  m_slope (0.0)
{
  clear ();
  load (points);
}

// Add a point to the curve.
void Vamos_Geometry::
Spline::load (const Two_Point& point)
{
  m_points.push_back (point);
  m_calculated = false;
}

// Add multiple points to the curve.
void Vamos_Geometry::
Spline::load (const std::vector <Two_Point>& points)
{
  for (std::vector <Two_Point>::const_iterator it = points.begin ();
       it != points.end ();
       it++)
    {
      m_points.push_back (*it);
    }
  m_calculated = false;
}

// Remove all points from the curve.
void Vamos_Geometry::
Spline::clear ()
{
  m_points.clear ();
  m_calculated = false;
}

// Remove points with x > LIMIT.
void Vamos_Geometry::
Spline::remove_greater (double limit)
{
  size_t size = 0;
  for (std::vector <Two_Point>::const_iterator it = m_points.begin ();
       it != m_points.end ();
       it++)
    {
      if (it->x > limit)
        {
          m_points.resize (size);
          break;
        }
      size++;
    }
  m_calculated = false;
}

// Scale all of the x values by FACTOR.
void Vamos_Geometry::
Spline::scale (double factor)
{
  for (std::vector <Two_Point>::iterator it = m_points.begin ();
       it != m_points.end ();
       it++)
    {
      it->x *= factor;
    }

  m_calculated = false;
}

// calculate() and interpolate() follow the discussion on cubic
// splines found in Numerical Recipes.  The implementation here is
// original. 

// Return the y value at the x value DISTANCE
double Vamos_Geometry::
Spline::interpolate (double distance) const
{
  if (m_points.size () == 1)
    {
      m_slope = 0.0;
      return m_points [0].y;
    }

  // calculate() only needs to be called once for a given set of
  // points.
  if (!m_calculated)
    calculate ();


  size_t low = 0;
  size_t high = m_points.size () - 1;
  size_t index;

  // Bisect to find the interval that distance is on.
  while ((high - low) > 1)
    {
      index = size_t ((high + low) / 2.0);
      if (m_points [index].x > distance)
        high = index;
      else
        low = index;
    }

  // Make sure that x_high > x_low.
  const double diff = m_points [high].x - m_points [low].x;
  assert (diff >= 0.0);

  // Evaluate the coefficients for the cubic spline equation.
  const double a = (m_points [high].x - distance) / diff;
  const double b = 1.0 - a;
  const double sq = diff*diff / 6.0;
  const double a2 = a*a;
  const double b2 = b*b;

  // Find the first derivitive.
  m_slope =
 (m_points [high].y - m_points [low].y)/diff
    - (3.0 * a2- 1.0) / 6.0 * diff * m_second_deriv [low]
    + (3.0 * b2 - 1.0) / 6.0 * diff * m_second_deriv [high];

  // Return the interpolated value.
  return a * m_points [low].y 
    + b * m_points [high].y 
    + a * (a2 - 1.0) * sq * m_second_deriv [low] 
    + b * (b2 - 1.0) * sq * m_second_deriv [high];
}


// Calculate the coefficients for interpolation.
void Vamos_Geometry::
Spline::calculate () const
{
  size_t n = m_points.size ();
  double* a = new double [n];
  double* b = new double [n];
  double* c = new double [n];
  double* r = new double [n];

  // Fill in the arrays that represent the tridiagonal matrix.
  // a [0] is not used. 
  double diff = m_points [1].x - m_points [0].x;
  b [0] = diff / 3.0;
  c [0] = diff / 6.0;
  r [0] = (m_points [1].y - m_points [0].y) / diff - m_first_slope;
    
  for (size_t i = 1; i < n - 1; i++)
    {
      double diff1 = m_points [i+1].x - m_points [i].x;
      double diff2 = m_points [i].x - m_points [i-1].x;

      a [i] = diff2 / 6.0;
      b [i] = (m_points [i+1].x - m_points [i-1].x) / 3.0;
      c [i] = diff1 / 6.0;
      r [i] = (m_points [i+1].y - m_points [i].y) / diff1
        - (m_points [i].y - m_points [i-1].y) / diff2;
    }

  diff = m_points [n-1].x - m_points [n-2].x;
  a [n-1] = diff / 6.0;
  b [n-1] = diff / 3.0;
  // c [n-1] is not used.
  r [n-1] = m_last_slope - (m_points [n-1].y - m_points [n-2]).y / diff;
    
    // Gauss-Jordan Elimination
    for (size_t i = 1; i < n; i++)
    {
        // Replace row i with row i - k * row (i-1) such that A_{i,i-1} = 0.0.
        double factor = a [i] / b [i-1];
        // A_{i,i-1} is not used again, so it need not be calculated.
        b [i] -= factor * c [i-1];
        // A_{i,i+1} is unchanged because A_{i-1,i+1} = 0.0.
        r [i] -= factor * r [i-1];
    }

    // Back-Substitution

    // Solve for y"[N].
    m_second_deriv.resize (n);
    m_second_deriv [n-1] = r [n-1] / b [n-1];
    for (int i = n - 2; i >= 0; i--)
    {
        // Use the solution for y"[i+1] to find y"[i].
        m_second_deriv [i] = (r [i] - c [i] * m_second_deriv [i+1]) / b [i];
    }

    delete [] r;
    delete [] c;
    delete [] b;
    delete [] a;

  m_calculated = true;
}

// Return the normal to the tanget at DISTANCE.
Vamos_Geometry::Two_Point Vamos_Geometry::
Spline::normal (double distance) const
{
  interpolate (distance);
  double theta = std::atan (m_slope);
  return Two_Point (-std::sin (theta), std::cos (theta));
}

---