Tools/Math/Interpol.cpp

Go to the documentation of this file.

//------------------------------------------------------------------------------
#include "Interpol.h"
#include "Tools/Math/LA_Matrix.h"
#include <algorithm>
//------------------------------------------------------------------------------
using namespace std;
//------------------------------------------------------------------------------
namespace interpol {
//------------------------------------------------------------------------------
Point::Point()
: x(0.0),
  y(0.0)
{
}
//------------------------------------------------------------------------------
Point::Point(double x, double y)
: x(x),
  y(y)
{
}
//------------------------------------------------------------------------------
bool operator<(const Point& p1, const Point& p2)
{
  return p1.x < p2.x;
}
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
Interpolation* InterpolationFactory::Create(Type type)
{
  switch (type)
  {
  case Linear:
    return new LinearInterpolation();
  case Spline:
    return new SplineInterpolation();
  default:
    return NULL;
  }
}
//------------------------------------------------------------------------------
Interpolation* InterpolationFactory::Create(Type type,
                                            const Interpolation* pSource)
{
  Interpolation* pInterpolation = Create(type);
  pInterpolation->Assign(*pSource);
  return pInterpolation;
}
//------------------------------------------------------------------------------
Interpolation* InterpolationFactory::Transform(Type type,
                                               Interpolation* pSource)
{
  Interpolation* pNewInterpolation = Create(type, pSource);
  Destroy(pSource);
  return pNewInterpolation;
}
//------------------------------------------------------------------------------
void InterpolationFactory::Destroy(Interpolation* pInterpolation)
{
  delete pInterpolation;
}
//------------------------------------------------------------------------------
void InterpolationFactory::Write(Out& out, const Interpolation* pInterpolation)
{
  out << pInterpolation->GetType();
  out << pInterpolation->points.size();
  std::map<int, interpol::Point>::const_iterator pos;
  for (pos = pInterpolation->points.begin();
       pos != pInterpolation->points.end();
       ++pos)
     out << pos->second;
}
//------------------------------------------------------------------------------
Interpolation* InterpolationFactory::Read(In& in)
{
  Type type;
  in >> (int&)type;
  Interpolation* pInterpolation = Create(type);

  int count;
  in >> count;
  for (int i = 1; i <= count; ++i)
  {
    Point p;
    in >> p;
    pInterpolation->points.insert(make_pair(i, p));
  }
  pInterpolation->nextIndex = count + 1;
  pInterpolation->BuildSortedPoints();
  pInterpolation->OnChange();
  return pInterpolation;
}
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
Interpolation::Interpolation()
: nextIndex(1)
{
}
//------------------------------------------------------------------------------
Interpolation::~Interpolation()
{
}
//------------------------------------------------------------------------------
int Interpolation::AddPoint(const Point& p)
{
  // Falls Punkt mit gleicher x-Koordinate bereits vorhanden -> Fehler
  if (binary_search(xSortedPoints.begin(), xSortedPoints.end(), p))
    return -1;

  // Punkt einfügen
  int id = nextIndex;
  points.insert(make_pair(id, p));
  ++nextIndex;

  // Sortierte Punkte aufbauen
  BuildSortedPoints();

  OnChange();
  
  return id;
}
//------------------------------------------------------------------------------
int Interpolation::AddPoint(double x, double y)
{
  return AddPoint(Point(x, y));
}
//------------------------------------------------------------------------------
bool Interpolation::MovePoint(int index, const Point& p)
{
  // Punkt mit Index suchen
  map<int, Point>::iterator ppos = points.find(index);
  if (ppos == points.end())
    return false;

  // Prüfen, ob Punkt mit gleicher x-Koordinate und anderem Index existiert
  map<int, Point>::const_iterator pos;
  for (pos = points.begin(); pos != points.end(); ++pos)
  {
    if (pos->first == index)
      continue;
    if (pos->second.x == p.x)
      return false;
  }

  // Punkt verschieben
  ppos->second = p;

  // Sortierte Punkte aufbauen
  BuildSortedPoints();

  OnChange();

  return true;
}
//------------------------------------------------------------------------------
bool Interpolation::RemovePoint(int index)
{
  // Punkt mit Index suchen
  map<int, Point>::iterator ppos = points.find(index);
  if (ppos == points.end())
    return false;

  // Punkt löschen
  points.erase(ppos);

  // Sortierte Punkte aufbauen
  BuildSortedPoints();

  OnChange();

  return true;
}
//------------------------------------------------------------------------------
Interpolation::ConstPointIterator Interpolation::GetPointBegin() const
{
  return points.begin();
}
//------------------------------------------------------------------------------
Interpolation::ConstPointIterator Interpolation::GetPointEnd() const
{
  return points.end();
}
//------------------------------------------------------------------------------
void Interpolation::Assign(const Interpolation& interpolation)
{
  xSortedPoints = interpolation.xSortedPoints;
  nextIndex = interpolation.nextIndex;
  points = interpolation.points;
  OnChange();
}
//------------------------------------------------------------------------------
void Interpolation::BuildSortedPoints()
{
  xSortedPoints.clear();
  map<int, Point>::const_iterator pos;
  for (pos = points.begin(); pos != points.end(); ++pos)
    xSortedPoints.push_back(pos->second);
  sort(xSortedPoints.begin(), xSortedPoints.end());
}
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
LinearInterpolation::LinearInterpolation()
: Interpolation()
{
}
//------------------------------------------------------------------------------
LinearInterpolation::~LinearInterpolation()
{
}
//------------------------------------------------------------------------------
double LinearInterpolation::GetInterpolatedY(double x) const
{
  // Prüfen, ob Punkte vorhanden sind
  int pointCount = (int)xSortedPoints.size();
  if (pointCount == 0)
    return 0.0;

  // Prüfen, ob Wert innerhalb der Grenzen liegen
  double leftX = xSortedPoints[0].x;
  double rightX = xSortedPoints[pointCount-1].x;
  if (x <= leftX)
    return xSortedPoints[0].y;
  if (x >= rightX)
    return xSortedPoints[pointCount-1].y;

  // Zum x-Wert benachbarte Punkte finden
  vector<Point>::const_iterator right =
    lower_bound(xSortedPoints.begin(), xSortedPoints.end(), Point(x, 0.0));
  if (right->x == x)
    return right->y;
  vector<Point>::const_iterator left = right-1;

  // Linear interpolieren
  double x1 = left->x;
  double y1 = left->y;
  double x2 = right->x;
  double y2 = right->y;
  return y1 + (x-x1)*(y2-y1)/(x2-x1);
}
//------------------------------------------------------------------------------
InterpolationFactory::Type LinearInterpolation::GetType() const
{
  return InterpolationFactory::Linear;
}
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
SplineInterpolation::SplineInterpolation()
: Interpolation()
{
}
//------------------------------------------------------------------------------
SplineInterpolation::~SplineInterpolation()
{
}
//------------------------------------------------------------------------------
double SplineInterpolation::GetInterpolatedY(double x) const
{
  if (this->x.dim() == 0)
    return 0.0;
  else if (this->x.dim() == 1)
    return y(1);
  
  if (x < this->x(1))
    return y(1);

  int i;
  for (i = 0; i < this->x.dim()-1; ++i)
  {
    if (x < this->x[i+1])
    {
      double r = x - this->x[i];
      double s = a[i] + b[i]*r + (c[i]/2.0)*r*r + (d[i]/6.0)*r*r*r;
      return s;
    }
  }
  
  return y(y.dim());
}
//------------------------------------------------------------------------------
InterpolationFactory::Type SplineInterpolation::GetType() const
{
  return InterpolationFactory::Spline;
}
//------------------------------------------------------------------------------
void SplineInterpolation::OnChange()
{
  CalcSplines();
}
//------------------------------------------------------------------------------
void SplineInterpolation::CalcSplines()
{
  int i;

  // Vektor mit x-Koordinaten ersellen
  x.create((int)xSortedPoints.size());
  y.create((int)xSortedPoints.size());
  for (i = 0; i < x.dim(); ++i)
  {
    x[i] = xSortedPoints[i].x;
    y[i] = xSortedPoints[i].y;
  }

  if (xSortedPoints.size() < 2)
    return;

  LA::Vector h(x.dim() - 1);
  for (i = 0; i < h.dim(); ++i)
    h[i] = x[i+1] - x[i];

  LA::Vector lambda(x.dim() - 1);
  for (i = 1; i < x.dim()-2; ++i)
    lambda[i] = h[i]/(h[i] + h[i-1]);

  LA::Vector mue(x.dim() - 1);
  for (i = 2; i < x.dim()-1; ++i)
    mue[i] = h[i-1]/(h[i] + h[i-1]);

  LA::Vector delta(x.dim() - 2);
  for (i = 1; i <= x.dim() - 2; ++i)
    delta(i) = 6.0/(h[i]+h[i-1]) * ((y[i+1]-y[i])/h[i] - (y[i]-y[i-1])/h[i-1]);
  
  LA::Matrix M(delta.dim(), delta.dim());
  for (i = 1; i <= delta.dim(); ++i)
  {
    M(i, i) = 2.0;
    if (i > 1)
      M(i, i-1) = mue[i];
    if (i < delta.dim())
      M(i, i+1) = lambda[i];
  }
  LA::Vector c_temp = M.solve(delta);

  c.create(x.dim());
  c.set_sub(2, c_temp);

  a.create(x.dim()-1);
  for (i = 0; i < a.dim(); ++i)
    a[i] = y[i];

  d.create(x.dim()-1);
  for (i = 0; i < d.dim(); ++i)
    d[i] = (c[i+1]-c[i])/h[i];

  b.create(x.dim()-1);
  for (i = 0; i < b.dim(); ++i)
    b[i] = (y[i+1]-y[i])/h[i] - h[i]/6.0*(2.0*c[i] + c[i+1]);

}
//------------------------------------------------------------------------------
Out& operator<<(Out& stream, const interpol::Point& p)
{
  stream << p.x;
  stream << p.y;
  return stream;
}
//------------------------------------------------------------------------------
In& operator>>(In& stream, interpol::Point& p)
{
  stream >> p.x;
  stream >> p.y;
  return stream;
}
//------------------------------------------------------------------------------
} // namespace interpol
//------------------------------------------------------------------------------

---