src/vamos/geometry/Linear_Interpolator.cc

Go to the documentation of this file.

// Linear_Interpolator.h - a piecewise-linear interpolator.
//
//      Vamos Automotive Simulator
//  Copyright (C) 2003 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/Linear_Interpolator.h>

#include <cmath>
#include <cassert>

Vamos_Geometry::
Linear_Interpolator::Linear_Interpolator () :
  m_delta_x (0.0),
  m_delta_y (0.0)
{
}

Vamos_Geometry::
Linear_Interpolator::
Linear_Interpolator (const std::vector <Two_Point>& points) :
  m_points (points),
  m_delta_x (0.0),
  m_delta_y (0.0)
{
}

void Vamos_Geometry::
Linear_Interpolator::load (const Two_Point& point)
{
  m_points.push_back (point);
}

void Vamos_Geometry::
Linear_Interpolator::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);
        }
}

void Vamos_Geometry::
Linear_Interpolator::clear ()
{
  m_points.clear ();
}

// Remove points with x > LIMIT.
void Vamos_Geometry::
Linear_Interpolator::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++;
        }
}

void Vamos_Geometry::
Linear_Interpolator::scale (double factor)
{
  for (std::vector <Two_Point>::iterator it = m_points.begin ();
           it != m_points.end ();
           it++)
        {
          it->x *= factor;
        }
}

double Vamos_Geometry::
Linear_Interpolator::interpolate (double dist) const
{
  assert (m_points.size () > 0);

  if (m_points.size () == 1)
        return m_points [0].y;

  if (dist > (m_points.end () - 1)->x)
        return (m_points.end () - 1)->y;
  if (dist < m_points.begin ()->x)
        return m_points.begin ()->y;

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

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

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

  m_delta_y = m_points [high].y - m_points [low].y;

  return m_points [low].y + m_delta_y * (dist - m_points [low].x) / m_delta_x;
}

Vamos_Geometry::Two_Point Vamos_Geometry::
Linear_Interpolator::normal (double dist) const
{
  interpolate (dist);
  double theta = std::atan2 (m_delta_y, m_delta_x);
  return Two_Point (-std::sin (theta), std::cos (theta));
}

---